h3n3/darkwater_cplus_640
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

big rejig test

Browse Source
This commit is contained in:
shrkey
2016-10-10 21:55:08 +01:00
parent 51004e9d4d
commit ab5dddccb2
2 changed files with 154 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
darkwater/DW640.cpp
View File

@@ -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;
}

63
darkwater/DW640.h
View File

@@ -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