void setMotorSpeed ( float speed ) {
unsigned int duty;
float temp;
float percent;
float dutyCyclePercent;
if ( speed > MOTOR_INPUT_MAX ) {
speed = MOTOR_INPUT_MAX;
} else if ( speed < MOTOR_INPUT_MIN ) {
speed = MOTOR_INPUT_MIN;
}
if ( speed >= 0 ) {
percent = speed / ( MOTOR_INPUT_MAX - MOTOR_INPUT_MID );
} else if ( speed < 0 ) {
percent = speed / ( MOTOR_INPUT_MID - MOTOR_INPUT_MIN );
}
if ( percent >= 0 ) {
dutyCyclePercent = percent * ( MOTOR_DUTY_CYCLE_MAX_PERCENT - MOTOR_DUTY_CYCLE_MID_PERCENT ) + MOTOR_DUTY_CYCLE_MID_PERCENT;
} else if ( percent < 0 ) {
dutyCyclePercent = percent * ( MOTOR_DUTY_CYCLE_MID_PERCENT - MOTOR_DUTY_CYCLE_MIN_PERCENT ) + MOTOR_DUTY_CYCLE_MID_PERCENT;
}
temp = ( ( dutyCyclePercent / 100.0 ) * ( MOTOR_DUTY_CYCLE_MAX - MOTOR_DUTY_CYCLE_MIN ) ) + MOTOR_DUTY_CYCLE_MIN;
duty = (unsigned int)temp;
setMotorPWM( duty );
}
void setMotorSpeedInt( unsigned int speed ) {
if( speed != 120 ) {
float fThrottle = (float)speed;
float setThrottle = 0.0;
float fPercent = 0.0;
unsigned int duty;
fPercent = fThrottle / 240.0;
setThrottle = fPercent*( 2370.0 - 1400.0 ) + 1400.0;
duty = (unsigned int)setThrottle;
setMotorPWM( duty );
}else{
setMotorPWM( 1885 );
}
}
void cn_debug_serial(uword command) {
sword pwm;
unsigned static short int motor = MOTOR3;
switch(command) {
case 'a':
setMotorDirection(motor, DIRECTION_CCW);
break;
case 's':
setMotorDirection(motor, DIRECTION_CW);
break;
case '1':
motor = MOTOR1;
break;
case '2':
motor = MOTOR2;
break;
case '3':
motor = MOTOR3;
break;
case 'u': // PWM up
pwm = getMotorPWM(motor);
pwm += 7;
setMotorPWM(motor, pwm);
break;
case 'd': // PWM down
pwm = getMotorPWM(motor);
pwm -= 7;
if(pwm < ZERO_PWM) pwm = ZERO_PWM;
setMotorPWM(motor, pwm);
break;
case 't': // toggle direction
toggleMotorDirection(motor);
break;
case ' ': // PWM off
stopAllMotors();
break;
default:
break;
}
}
void updateMotor( uint16_t top, uint16_t normalizedDuty, uint8_t vccx10 ) {
unsigned long voltageAdjustedDuty = (((normalizedDuty * 42UL ) / vccx10) ); // All dutys are normalized to 4.2 volts, so adjust to the current volatge level. Note that is could overflow an uint16 if the voltage is lower than the normal value.
unsigned long voltageAdjusedMatch = (voltageAdjustedDuty * top ) / 65535UL; // Covert the duty that is scaled 0-65535 to a match that is scaled 0-top.
// Match = (duty/65535) * top, but we need to stay integer so switch the order
// Keep as a long because it could be bigger than an int due to scaling because of a low voltage
uint16_t match;
if (voltageAdjusedMatch > top ) { // Battery to low for requested duty, so give it all we've got
match = top;
} else {
match = (uint16_t) voltageAdjusedMatch; // We know that adjusted duty will fit into uint_16 here because it is less than top which is a uint16
}
setMotorPWM( match , top );
}
void stopMotor(ubyte motor) {
setMotorPWM(motor, ZERO_PWM);
}
