From ab5dddccb2bc6db9c7d219fa6dbbeec96cbf2bac Mon Sep 17 00:00:00 2001 From: shrkey Date: Mon, 10 Oct 2016 21:55:08 +0100 Subject: [PATCH] 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