From ab5dddccb2bc6db9c7d219fa6dbbeec96cbf2bac Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 21:55:08 +0100 Subject: [PATCH 01/12] big rejig test --- darkwater/DW640.cpp | 173 ++++++++++++++++++++++++++------------------ darkwater/DW640.h | 63 +++++++++++++--- 2 files changed, 154 insertions(+), 82 deletions(-) diff --git a/darkwater/DW640.cpp b/darkwater/DW640.cpp index cee56b9..24d952b 100644 --- a/darkwater/DW640.cpp +++ b/darkwater/DW640.cpp @@ -196,7 +196,7 @@ void DW640::allOff() { /* DC Motor specific code */ -void DW640::setMotorSpeed(uint8_t motor, int16_t speed) { +DW_Motor *DW640::getMotor(uint8_t motor) { uint8_t in1; uint8_t in2; @@ -230,125 +230,158 @@ void DW640::setMotorSpeed(uint8_t motor, int16_t speed) { default: fprintf(stderr, "Motor number must be between 1 and 6 inclusive"); } + + if(motors[motor].motor == 0) { + // We don't have one yet + motors[motor].motor = motor; // how many motors on one line?!? + motors[motor].DWC = this; + + motors[motor].in1 = in1; + motors[motor].in2 = in2; + } + + return &motors[motor]; + +} + +DW_Servo *DW640::getServo(uint8_t servo) { + + uint8_t pin; + + // Get the servo + switch(servo) { + case 1: + pin = 0; // Servo 1 is on PWM 0 + break; + case 2: + pin = 1; // Servo 2 is on PWM 1 + break; + default: + fprintf(stderr, "Servo number must be between 1 and 2 inclusive"); + } + + if(servos[servo].servo == 0) { + // We don't have one yet + servos[servo].servo = servo; // how many servos on one line?!? + servos[servo].DWC = this; + + servos[servo].pin = pin; + } + + return &servos[servo]; + +} + +/* Motors code */ + +DW_Motor::DW_Motor(void) { + DWC = NULL; + motor = 0; + in1 = in2 = 0; +} + +void DW_Motor::setMotorSpeed(int16_t speed) { + // Speed deciphering for the two control modes if( speed >= 1000 && speed < 1500 ) { - runMotor( DW_REVERSE, in1, in2, map(speed, 1500, 1000, 0, 255 ) ); + run( DW_REVERSE, in1, in2, map(speed, 1500, 1000, 0, 255 ) ); } else if( speed > 1500 && speed <= 2000 ) { - runMotor( DW_FORWARD, in1, in2, map(speed, 1500, 2000, 0, 255 ) ); + run( DW_FORWARD, in1, in2, map(speed, 1500, 2000, 0, 255 ) ); } else if( speed > 0 && speed <= 255 ) { - runMotor( DW_FORWARD, in1, in2, speed ); + run( DW_FORWARD, in1, in2, speed ); } else if( speed < 0 && speed >= -255 ) { - runMotor( DW_REVERSE, in1, in2, abs(speed) ); + run( DW_REVERSE, in1, in2, abs(speed) ); } else if( speed == 0 || speed == 1500 ) { - runMotor( DW_STOP, in1, in2, speed ); + run( DW_STOP, in1, in2, speed ); } } -void DW640::setMotorOff(uint8_t motor) { - setMotorSpeed( motor, 0 ); +void DW_Motor::off(void) { + setMotorSpeed( 0 ); } -void DW640::runMotor( uint8_t control, uint8_t in1, uint8_t in2, uint16_t speed ) { +void DW_Motor::run( uint8_t control, uint8_t in1, uint8_t in2, uint16_t speed ) { // get the mode - if( getMode() == DW_PHASE ) { + if( DWC->getMode() == DW_PHASE ) { if( control == DW_FORWARD ) { - setPin( in2, 0 ); - setPWM( in1, 0, speed * 16 ); + DWC->setPin( in2, 0 ); + DWC->setPWM( in1, 0, speed * 16 ); } else if( control == DW_REVERSE ) { - setPin( in2, 1 ); - setPWM( in1, 0, speed * 16 ); + DWC->setPin( in2, 1 ); + DWC->setPWM( in1, 0, speed * 16 ); } else if( control == DW_STOP ) { - setPin( in2, 0 ); - setPin( in1, 0 ); + DWC->setPin( in2, 0 ); + DWC->setPin( in1, 0 ); } } else { // DW_ININ if( control == DW_FORWARD ) { - setPin( in2, 0 ); - setPWM( in1, 0, speed * 16 ); + DWC->setPin( in2, 0 ); + DWC->setPWM( in1, 0, speed * 16 ); } else if( control == DW_REVERSE ) { - setPin( in1, 0 ); - setPWM( in2, 0, speed * 16 ); + DWC->setPin( in1, 0 ); + DWC->setPWM( in2, 0, speed * 16 ); } else if( control == DW_STOP ) { - setPin( in1, 1 ); - setPin( in2, 1 ); + DWC->setPin( in1, 1 ); + DWC->setPin( in2, 1 ); } else if( control == DW_COAST ) { - setPin( in1, 0 ); - setPin( in2, 0 ); + DWC->setPin( in1, 0 ); + DWC->setPin( in2, 0 ); } } } +uint16_t DW_Motor::map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + /* Servo code */ -void DW640::setServoOff(uint8_t servo) { +DW_Servo::DW_Servo(void) { + DWC = NULL; + servo = 0; + pin = 0; +} - switch(servo) { - case 1: - setPin( 0, 0 ); // Servo 1 is on PWM 0 - break; - case 2: - setPin( 1, 0 ); // Servo 2 is on PWM 1 - break; - default: - fprintf(stderr, "Servo number must be between 1 and 2 inclusive"); - } +void DW_Servo::off(void) { + + DWC->SetPin( pin, 0 ); } -void DW640::setServoPWMmS(uint8_t servo, float length_mS) { +void DW_Servo::setPWMmS(float length_mS) { - switch(servo) { - case 1: - setPWMmS( 0, length_mS ); // Servo 1 is on PWM 0 - break; - case 2: - setPWMmS( 1, length_mS ); // Servo 2 is on PWM 1 - break; - default: - fprintf(stderr, "Servo number must be between 1 and 2 inclusive"); - } + DWC->setPWMmS( pin, length_mS ); // Servo 1 is on PWM 0 } -void DW640::setServoPWMuS(uint8_t servo, float length_uS) { +void DW_Servo::setPWMuS(float length_uS) { - switch(servo) { - case 1: - setPWMuS( 0, length_uS ); // Servo 1 is on PWM 0 - break; - case 2: - setPWMuS( 1, length_uS ); // Servo 2 is on PWM 1 - break; - default: - fprintf(stderr, "Servo number must be between 1 and 2 inclusive"); - } + DWC->setPWMuS( pin, length_uS ); // Servo 1 is on PWM 0 + } /* Stepper functions */ -void DW640::setStepperOff(uint8_t stepper) { +// void DW640::setStepperOff(uint8_t stepper) { -} +// } -void DW640::setStepperSpeed(uint8_t stepper, uint16_t speed) { +// void DW640::setStepperSpeed(uint8_t stepper, uint16_t speed) { -} +// } -void DW640::oneStep(uint8_t stepper, uint8_t direction, uint8_t style) { +// void DW640::oneStep(uint8_t stepper, uint8_t direction, uint8_t style) { -} +// } -void DW640::step(uint8_t stepper, uint16_t steps, uint8_t direction, uint8_t style) { - -} +// void DW640::step(uint8_t stepper, uint16_t steps, uint8_t direction, uint8_t style) { + +// } /* Private functions */ -uint16_t DW640::map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max) -{ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} \ No newline at end of file diff --git a/darkwater/DW640.h b/darkwater/DW640.h index b78c340..8c5a61b 100644 --- a/darkwater/DW640.h +++ b/darkwater/DW640.h @@ -59,6 +59,40 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace DarkWater; +class DW_Motor { + public: + DW_Motor(void); + friend class DW640; + + void setMotorSpeed(int16_t speed); + void off(void); + void run(uint8_t control, uint16_t speed ); + + private: + uint8_t in1, in2; + DW640 *DWC; + uint8_t motor; + + uint16_t map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max); + +} + +class DW_Servo { + public: + DW_Servo(void); + friend class DW640; + + void off(void); + void setPWMmS(float length_mS); + void setPWMuS(float length_uS); + + private: + uint8_t pin; + DW640 *DWC; + uint8_t servo; + +} + class DW640 { public: DW640(uint8_t address = DW640_DEFAULT_ADDRESS); @@ -87,18 +121,13 @@ class DW640 { void allOff(); - void setMotorSpeed(uint8_t motor, int16_t speed); - void setMotorOff(uint8_t motor); - void runMotor( uint8_t control, uint8_t in1, uint8_t in2, uint16_t speed ); + DW_Motor *getMotor(uint8_t motor); + DW_Servo *getServo(uint8_t servo); - void setServoOff(uint8_t servo); - void setServoPWMmS(uint8_t servo, float length_mS); - void setServoPWMuS(uint8_t servo, float length_uS); - - void setStepperOff(uint8_t stepper); - void setStepperSpeed(uint8_t stepper, uint16_t speed); - void oneStep(uint8_t stepper, uint8_t direction, uint8_t style); - void step(uint8_t stepper, uint16_t steps, uint8_t direction, uint8_t style = DW_SINGLE); + // void setStepperOff(uint8_t stepper); + // void setStepperSpeed(uint8_t stepper, uint16_t speed); + // void oneStep(uint8_t stepper, uint8_t direction, uint8_t style); + // void step(uint8_t stepper, uint16_t steps, uint8_t direction, uint8_t style = DW_SINGLE); private: uint8_t devAddr; @@ -107,9 +136,19 @@ class DW640 { PCA9685* pwm; Pin* modePin; - uint16_t map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max); + DW_Motor motors[6]; + DW_Servo servos[2]; + + uint16_t revsteps; // # steps per revolution + uint8_t currentstep; + + }; +// class DW_Stepper { + +// } + #endif // DW640_H \ No newline at end of file From c007b870e53834ded12ff812538e851b8a854f38 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 21:57:26 +0100 Subject: [PATCH 02/12] motor test --- examples/Motor/Motor.cpp | 147 ++++++++++++++++++++------------------- 1 file changed, 77 insertions(+), 70 deletions(-) diff --git a/examples/Motor/Motor.cpp b/examples/Motor/Motor.cpp index 5de6375..c0579bd 100644 --- a/examples/Motor/Motor.cpp +++ b/examples/Motor/Motor.cpp @@ -27,77 +27,84 @@ int main() DW640 dw; dw.initialize(); - dw.setMotorOff(1); - dw.setMotorOff(2); - dw.setMotorOff(3); - dw.setMotorOff(4); - dw.setMotorOff(5); - dw.setMotorOff(6); - usleep(1000000); + DW_Motor *dw1 = dw.getMotor(1); - printf("Set forward - \n"); - printf("Motor 1\n"); - dw.setMotorSpeed( 1, 255 ); + dw1.off(); + dw1.setMotorSpeed( 200 ); usleep(1000000); - printf("Motor 2\n"); - dw.setMotorSpeed( 2, 255 ); - usleep(1000000); - printf("Motor 3\n"); - dw.setMotorSpeed( 3, 255 ); - usleep(1000000); - printf("Motor 4\n"); - dw.setMotorSpeed( 4, 255 ); - usleep(1000000); - printf("Motor 5\n"); - dw.setMotorSpeed( 5, 255 ); - usleep(1000000); - printf("Motor 6\n"); - dw.setMotorSpeed( 6, 255 ); - usleep(1000000); - printf("Stopping - \n"); - printf("Motor 1\n"); - dw.setMotorSpeed( 1, 0 ); - usleep(1000000); - printf("Motor 2\n"); - dw.setMotorSpeed( 2, 0 ); - usleep(1000000); - printf("Motor 3\n"); - dw.setMotorSpeed( 3, 0 ); - usleep(1000000); - printf("Motor 4\n"); - dw.setMotorSpeed( 4, 0 ); - usleep(1000000); - printf("Motor 5\n"); - dw.setMotorSpeed( 5, 0 ); - usleep(1000000); - printf("Motor 6\n"); - dw.setMotorSpeed( 6, 0 ); - usleep(1000000); - printf("Set reverse - \n"); - printf("Motor 1\n"); - dw.setMotorSpeed( 1, -255 ); - usleep(1000000); - printf("Motor 2\n"); - dw.setMotorSpeed( 2, -255 ); - usleep(1000000); - printf("Motor 3\n"); - dw.setMotorSpeed( 3, -255 ); - usleep(1000000); - printf("Motor 4\n"); - dw.setMotorSpeed( 4, -255 ); - usleep(1000000); - printf("Motor 5\n"); - dw.setMotorSpeed( 5, -255 ); - usleep(1000000); - printf("Motor 6\n"); - dw.setMotorSpeed( 6, -255 ); - usleep(1000000); - printf("All off \n"); - dw.setMotorOff(1); - dw.setMotorOff(2); - dw.setMotorOff(3); - dw.setMotorOff(4); - dw.setMotorOff(5); - dw.setMotorOff(6); + dw1.off(); + + // dw.setMotorOff(1); + // dw.setMotorOff(2); + // dw.setMotorOff(3); + // dw.setMotorOff(4); + // dw.setMotorOff(5); + // dw.setMotorOff(6); + // usleep(1000000); + + // printf("Set forward - \n"); + // printf("Motor 1\n"); + // dw.setMotorSpeed( 1, 255 ); + // usleep(1000000); + // printf("Motor 2\n"); + // dw.setMotorSpeed( 2, 255 ); + // usleep(1000000); + // printf("Motor 3\n"); + // dw.setMotorSpeed( 3, 255 ); + // usleep(1000000); + // printf("Motor 4\n"); + // dw.setMotorSpeed( 4, 255 ); + // usleep(1000000); + // printf("Motor 5\n"); + // dw.setMotorSpeed( 5, 255 ); + // usleep(1000000); + // printf("Motor 6\n"); + // dw.setMotorSpeed( 6, 255 ); + // usleep(1000000); + // printf("Stopping - \n"); + // printf("Motor 1\n"); + // dw.setMotorSpeed( 1, 0 ); + // usleep(1000000); + // printf("Motor 2\n"); + // dw.setMotorSpeed( 2, 0 ); + // usleep(1000000); + // printf("Motor 3\n"); + // dw.setMotorSpeed( 3, 0 ); + // usleep(1000000); + // printf("Motor 4\n"); + // dw.setMotorSpeed( 4, 0 ); + // usleep(1000000); + // printf("Motor 5\n"); + // dw.setMotorSpeed( 5, 0 ); + // usleep(1000000); + // printf("Motor 6\n"); + // dw.setMotorSpeed( 6, 0 ); + // usleep(1000000); + // printf("Set reverse - \n"); + // printf("Motor 1\n"); + // dw.setMotorSpeed( 1, -255 ); + // usleep(1000000); + // printf("Motor 2\n"); + // dw.setMotorSpeed( 2, -255 ); + // usleep(1000000); + // printf("Motor 3\n"); + // dw.setMotorSpeed( 3, -255 ); + // usleep(1000000); + // printf("Motor 4\n"); + // dw.setMotorSpeed( 4, -255 ); + // usleep(1000000); + // printf("Motor 5\n"); + // dw.setMotorSpeed( 5, -255 ); + // usleep(1000000); + // printf("Motor 6\n"); + // dw.setMotorSpeed( 6, -255 ); + // usleep(1000000); + // printf("All off \n"); + // dw.setMotorOff(1); + // dw.setMotorOff(2); + // dw.setMotorOff(3); + // dw.setMotorOff(4); + // dw.setMotorOff(5); + // dw.setMotorOff(6); } From adff66be2ece41f3dff641f51e5ca78d40ed8b53 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:01:55 +0100 Subject: [PATCH 03/12] forward declaration --- darkwater/DW640.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/darkwater/DW640.h b/darkwater/DW640.h index 8c5a61b..c9fcfa1 100644 --- a/darkwater/DW640.h +++ b/darkwater/DW640.h @@ -59,6 +59,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace DarkWater; +class DW640; + class DW_Motor { public: DW_Motor(void); From 35f3423400410c1a776928c9aeec448930c35069 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:03:58 +0100 Subject: [PATCH 04/12] doh --- darkwater/DW640.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/darkwater/DW640.h b/darkwater/DW640.h index c9fcfa1..b7c62ca 100644 --- a/darkwater/DW640.h +++ b/darkwater/DW640.h @@ -77,7 +77,7 @@ class DW_Motor { uint16_t map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max); -} +}; class DW_Servo { public: @@ -93,7 +93,7 @@ class DW_Servo { DW640 *DWC; uint8_t servo; -} +}; class DW640 { public: From a33420d16568e602a3ba1fee785912bda5c3a97a Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:06:32 +0100 Subject: [PATCH 05/12] changes --- examples/Motor/Motor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/Motor/Motor.cpp b/examples/Motor/Motor.cpp index c0579bd..5947d1f 100644 --- a/examples/Motor/Motor.cpp +++ b/examples/Motor/Motor.cpp @@ -29,10 +29,10 @@ int main() DW_Motor *dw1 = dw.getMotor(1); - dw1.off(); - dw1.setMotorSpeed( 200 ); + dw1->off(); + dw1->setMotorSpeed( 200 ); usleep(1000000); - dw1.off(); + dw1->off(); // dw.setMotorOff(1); // dw.setMotorOff(2); From d30c7279b6031a8161a436949a0096ef6b87e849 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:08:21 +0100 Subject: [PATCH 06/12] rejig run --- darkwater/DW640.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/darkwater/DW640.cpp b/darkwater/DW640.cpp index 24d952b..08a18b6 100644 --- a/darkwater/DW640.cpp +++ b/darkwater/DW640.cpp @@ -284,15 +284,15 @@ void DW_Motor::setMotorSpeed(int16_t speed) { // Speed deciphering for the two control modes if( speed >= 1000 && speed < 1500 ) { - run( DW_REVERSE, in1, in2, map(speed, 1500, 1000, 0, 255 ) ); + run( DW_REVERSE, map(speed, 1500, 1000, 0, 255 ) ); } else if( speed > 1500 && speed <= 2000 ) { - run( DW_FORWARD, in1, in2, map(speed, 1500, 2000, 0, 255 ) ); + run( DW_FORWARD, map(speed, 1500, 2000, 0, 255 ) ); } else if( speed > 0 && speed <= 255 ) { - run( DW_FORWARD, in1, in2, speed ); + run( DW_FORWARD, speed ); } else if( speed < 0 && speed >= -255 ) { - run( DW_REVERSE, in1, in2, abs(speed) ); + run( DW_REVERSE, abs(speed) ); } else if( speed == 0 || speed == 1500 ) { - run( DW_STOP, in1, in2, speed ); + run( DW_STOP, speed ); } @@ -303,7 +303,7 @@ void DW_Motor::off(void) { setMotorSpeed( 0 ); } -void DW_Motor::run( uint8_t control, uint8_t in1, uint8_t in2, uint16_t speed ) { +void DW_Motor::run( uint8_t control, uint16_t speed ) { // get the mode if( DWC->getMode() == DW_PHASE ) { if( control == DW_FORWARD ) { From 774de67bf749f98a3d71909f72b28f44605eabc1 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:09:16 +0100 Subject: [PATCH 07/12] capitals doh --- darkwater/DW640.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/darkwater/DW640.cpp b/darkwater/DW640.cpp index 08a18b6..b757f52 100644 --- a/darkwater/DW640.cpp +++ b/darkwater/DW640.cpp @@ -348,7 +348,7 @@ DW_Servo::DW_Servo(void) { void DW_Servo::off(void) { - DWC->SetPin( pin, 0 ); + DWC->setPin( pin, 0 ); } From de71233303f08460c8279f21c38649e59e8869bc Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:12:31 +0100 Subject: [PATCH 08/12] servo test --- examples/Servo/Servo.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/examples/Servo/Servo.cpp b/examples/Servo/Servo.cpp index 45b50ea..a8dae4e 100644 --- a/examples/Servo/Servo.cpp +++ b/examples/Servo/Servo.cpp @@ -18,20 +18,23 @@ int main() dw.initialize(); dw.setFrequency(50); - dw.setServoOff( 1 ); - dw.setServoOff( 2 ); + DW_Servo *s1 = dw.getServo(1); + DW_Servo *s2 = dw.getServo(2); + + s1->off(); + s2->off(); for( int a = 10; a >= 0; a-- ) { - dw.setServoPWMmS(1, SERVO_MIN); - dw.setServoPWMmS(2, SERVO_MIN); + s1->setPWMmS(SERVO_MIN); + s2->setPWMmS(SERVO_MIN); usleep(1000000); - dw.setServoPWMmS(1, SERVO_MAX); - dw.setServoPWMmS(2, SERVO_MAX); + s1->setPWMmS(SERVO_MAX); + s2->setPWMmS(SERVO_MAX); usleep(1000000); } - dw.setServoOff( 1 ); - dw.setServoOff( 2 ); + s1->off(); + s2->off(); return 0; } From 69384be0d00934bdcfb589a126ff8becf43d7045 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:14:14 +0100 Subject: [PATCH 09/12] debug --- examples/Servo/Servo.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/Servo/Servo.cpp b/examples/Servo/Servo.cpp index a8dae4e..8f8c0a7 100644 --- a/examples/Servo/Servo.cpp +++ b/examples/Servo/Servo.cpp @@ -24,7 +24,9 @@ int main() s1->off(); s2->off(); + printf("Start servo moves\n"); for( int a = 10; a >= 0; a-- ) { + printf("Step %d\n", a); s1->setPWMmS(SERVO_MIN); s2->setPWMmS(SERVO_MIN); usleep(1000000); From b2e758c50b37d5279ffdf030984ce862a0fee584 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:18:16 +0100 Subject: [PATCH 10/12] new rejig --- darkwater/DW640.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/darkwater/DW640.cpp b/darkwater/DW640.cpp index b757f52..e9052c4 100644 --- a/darkwater/DW640.cpp +++ b/darkwater/DW640.cpp @@ -201,29 +201,31 @@ DW_Motor *DW640::getMotor(uint8_t motor) { uint8_t in1; uint8_t in2; + motor--; + // Get the motor switch(motor) { - case 1: + case 0: in2 = 2; in1 = 3; break; - case 2: + case 1: in2 = 4; in1 = 5; break; - case 3: + case 2: in2 = 6; in1 = 7; break; - case 4: + case 3: in2 = 8; in1 = 9; break; - case 5: + case 4: in2 = 10; in1 = 11; break; - case 6: + case 5: in2 = 12; in1 = 13; break; @@ -248,12 +250,14 @@ DW_Servo *DW640::getServo(uint8_t servo) { uint8_t pin; + servo--; + // Get the servo switch(servo) { - case 1: + case 0: pin = 0; // Servo 1 is on PWM 0 break; - case 2: + case 1: pin = 1; // Servo 2 is on PWM 1 break; default: From ef3b6ab9446376e3f8843d3b0b04867cf085051e Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:25:24 +0100 Subject: [PATCH 11/12] new motor example --- examples/Motor/Motor.cpp | 147 ++++++++++++++++++++------------------- 1 file changed, 76 insertions(+), 71 deletions(-) diff --git a/examples/Motor/Motor.cpp b/examples/Motor/Motor.cpp index 5947d1f..7e41c6f 100644 --- a/examples/Motor/Motor.cpp +++ b/examples/Motor/Motor.cpp @@ -28,83 +28,88 @@ int main() dw.initialize(); DW_Motor *dw1 = dw.getMotor(1); + DW_Motor *dw2 = dw.getMotor(2); + DW_Motor *dw3 = dw.getMotor(3); + DW_Motor *dw4 = dw.getMotor(4); + DW_Motor *dw5 = dw.getMotor(5); + DW_Motor *dw6 = dw.getMotor(6); dw1->off(); dw1->setMotorSpeed( 200 ); usleep(1000000); dw1->off(); - // dw.setMotorOff(1); - // dw.setMotorOff(2); - // dw.setMotorOff(3); - // dw.setMotorOff(4); - // dw.setMotorOff(5); - // dw.setMotorOff(6); - // usleep(1000000); + dw1->off(); + dw2->off(); + dw3->off(); + dw4->off(); + dw5->off(); + dw6->off(); + usleep(1000000); - // printf("Set forward - \n"); - // printf("Motor 1\n"); - // dw.setMotorSpeed( 1, 255 ); - // usleep(1000000); - // printf("Motor 2\n"); - // dw.setMotorSpeed( 2, 255 ); - // usleep(1000000); - // printf("Motor 3\n"); - // dw.setMotorSpeed( 3, 255 ); - // usleep(1000000); - // printf("Motor 4\n"); - // dw.setMotorSpeed( 4, 255 ); - // usleep(1000000); - // printf("Motor 5\n"); - // dw.setMotorSpeed( 5, 255 ); - // usleep(1000000); - // printf("Motor 6\n"); - // dw.setMotorSpeed( 6, 255 ); - // usleep(1000000); - // printf("Stopping - \n"); - // printf("Motor 1\n"); - // dw.setMotorSpeed( 1, 0 ); - // usleep(1000000); - // printf("Motor 2\n"); - // dw.setMotorSpeed( 2, 0 ); - // usleep(1000000); - // printf("Motor 3\n"); - // dw.setMotorSpeed( 3, 0 ); - // usleep(1000000); - // printf("Motor 4\n"); - // dw.setMotorSpeed( 4, 0 ); - // usleep(1000000); - // printf("Motor 5\n"); - // dw.setMotorSpeed( 5, 0 ); - // usleep(1000000); - // printf("Motor 6\n"); - // dw.setMotorSpeed( 6, 0 ); - // usleep(1000000); - // printf("Set reverse - \n"); - // printf("Motor 1\n"); - // dw.setMotorSpeed( 1, -255 ); - // usleep(1000000); - // printf("Motor 2\n"); - // dw.setMotorSpeed( 2, -255 ); - // usleep(1000000); - // printf("Motor 3\n"); - // dw.setMotorSpeed( 3, -255 ); - // usleep(1000000); - // printf("Motor 4\n"); - // dw.setMotorSpeed( 4, -255 ); - // usleep(1000000); - // printf("Motor 5\n"); - // dw.setMotorSpeed( 5, -255 ); - // usleep(1000000); - // printf("Motor 6\n"); - // dw.setMotorSpeed( 6, -255 ); - // usleep(1000000); - // printf("All off \n"); - // dw.setMotorOff(1); - // dw.setMotorOff(2); - // dw.setMotorOff(3); - // dw.setMotorOff(4); - // dw.setMotorOff(5); - // dw.setMotorOff(6); + printf("Set forward - \n"); + printf("Motor 1\n"); + dw1->setMotorSpeed( 255 ); + usleep(1000000); + printf("Motor 2\n"); + dw2->setMotorSpeed( 255 ); + usleep(1000000); + printf("Motor 3\n"); + dw3->setMotorSpeed( 255 ); + usleep(1000000); + printf("Motor 4\n"); + dw4->setMotorSpeed( 255 ); + usleep(1000000); + printf("Motor 5\n"); + dw5->setMotorSpeed( 255 ); + usleep(1000000); + printf("Motor 6\n"); + dw6->setMotorSpeed( 255 ); + usleep(1000000); + printf("Stopping - \n"); + printf("Motor 1\n"); + dw1->setMotorSpeed( 0 ); + usleep(1000000); + printf("Motor 2\n"); + dw2->setMotorSpeed( 0 ); + usleep(1000000); + printf("Motor 3\n"); + dw3->setMotorSpeed( 0 ); + usleep(1000000); + printf("Motor 4\n"); + dw4->setMotorSpeed( 0 ); + usleep(1000000); + printf("Motor 5\n"); + dw5->setMotorSpeed( 0 ); + usleep(1000000); + printf("Motor 6\n"); + dw6->setMotorSpeed( 0 ); + usleep(1000000); + printf("Set reverse - \n"); + printf("Motor 1\n"); + dw1->setMotorSpeed( -255 ); + usleep(1000000); + printf("Motor 2\n"); + dw2->setMotorSpeed( -255 ); + usleep(1000000); + printf("Motor 3\n"); + dw3->setMotorSpeed( -255 ); + usleep(1000000); + printf("Motor 4\n"); + dw4->setMotorSpeed( -255 ); + usleep(1000000); + printf("Motor 5\n"); + dw5->setMotorSpeed( -255 ); + usleep(1000000); + printf("Motor 6\n"); + dw6->setMotorSpeed( -255 ); + usleep(1000000); + printf("All off \n"); + dw1->off(); + dw2->off(); + dw3->off(); + dw4->off(); + dw5->off(); + dw6->off(); } From 8bb075a3c59d57ea6936faefd3855e667662d4e7 Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 22:26:56 +0100 Subject: [PATCH 12/12] remove test --- examples/Motor/Motor.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/examples/Motor/Motor.cpp b/examples/Motor/Motor.cpp index 7e41c6f..9e8b41b 100644 --- a/examples/Motor/Motor.cpp +++ b/examples/Motor/Motor.cpp @@ -34,11 +34,6 @@ int main() DW_Motor *dw5 = dw.getMotor(5); DW_Motor *dw6 = dw.getMotor(6); - dw1->off(); - dw1->setMotorSpeed( 200 ); - usleep(1000000); - dw1->off(); - dw1->off(); dw2->off(); dw3->off();