Practical Robotics Modular Robot Library
Diff: motors.cpp
- Revision:
- 6:732aa91eb555
- Parent:
- 5:6da8daaeb9f7
diff -r 6da8daaeb9f7 -r 732aa91eb555 motors.cpp --- a/motors.cpp Mon Jan 02 22:56:34 2017 +0000 +++ b/motors.cpp Fri Jan 13 23:16:23 2017 +0000 @@ -17,128 +17,156 @@ float Motors::get_left_motor_speed() { - return left_motor_speed; + return left_motor_speed; } float Motors::get_right_motor_speed() { - return right_motor_speed; + return right_motor_speed; } -void Motors::forwards(float speed){ +void Motors::forwards(float speed) +{ set_left_motor_speed(speed); - set_right_motor_speed(speed); + set_right_motor_speed(speed); } -void Motors::backwards(float speed){ +void Motors::backwards(float speed) +{ set_left_motor_speed(-speed); - set_right_motor_speed(-speed); + set_right_motor_speed(-speed); } -void Motors::turn(float speed){ +void Motors::turn(float speed) +{ set_left_motor_speed(speed); - set_right_motor_speed(-speed); + set_right_motor_speed(-speed); } -float Motors::get_current_left(){ +float Motors::get_current_left() +{ float current = motor_vprop_l.read(); //Vprop = 5x voltage on sense pins - sense pins have 0.25 ohm resistance - current is 0.8 actual voltage measured [or 2.64 x scaled value] - return current * 2.64f; + return current * 2.64f; } -float Motors::get_current_right(){ +float Motors::get_current_right() +{ float current = motor_vprop_r.read(); - return current * 2.64f; + return current * 2.64f; } -float Motors::get_adjusted_speed(float speed_in){ +float Motors::get_adjusted_speed(float speed_in) +{ float ret_speed = speed_in; - if(USE_STALL_OFFSET == 1){ + if(USE_STALL_OFFSET == 1) { ret_speed += STALL_OFFSET; - ret_speed /= (STALL_OFFSET + 1.0f); - } + ret_speed /= (STALL_OFFSET + 1.0f); + } return ret_speed; } -void Motors::sleep(){ - nsleep = 0; +void Motors::sleep() +{ + nsleep = 0; left_motor_speed = 0; right_motor_speed = 0; } -void Motors::wake_up(){ - nsleep = 1; +void Motors::wake_up() +{ + nsleep = 1; } -void Motors::coast_left(){ - enable_l = 0; +void Motors::coast_left() +{ + enable_l = 0; left_motor_speed = 0; } -void Motors::brake_left(){ +void Motors::brake_left() +{ hb1_l = 1; hb2_l = 1; - enable_l = 1; + enable_l = 1; left_motor_speed = 0; } -void Motors::set_left_motor_speed(float speed){ - if(speed < 0){ - hb1_l = 0; - hb2_l = 1; - left_motor_speed = -get_adjusted_speed(-speed); - enable_l = -left_motor_speed; - }else{ - hb1_l = 1; - hb2_l = 0; - left_motor_speed = get_adjusted_speed(speed); - enable_l = left_motor_speed; - } +void Motors::set_left_motor_speed(float speed) +{ + if(speed == 0) { + left_motor_speed = 0; + enable_l = 0; + } else { + if(speed < 0) { + hb1_l = 1; + hb2_l = 0; + left_motor_speed = -get_adjusted_speed(-speed); + enable_l = -left_motor_speed; + } else { + hb1_l = 0; + hb2_l = 1; + left_motor_speed = get_adjusted_speed(speed); + enable_l = left_motor_speed; + } + } } -void Motors::coast_right(){ - enable_r = 0; +void Motors::coast_right() +{ + enable_r = 0; right_motor_speed = 0; } -void Motors::brake_right(){ +void Motors::brake_right() +{ hb1_r = 1; hb2_r = 1; - enable_r = 1; + enable_r = 1; right_motor_speed = 0; } -void Motors::coast(){ +void Motors::coast() +{ coast_left(); - coast_right(); + coast_right(); } -void Motors::brake(){ +void Motors::brake() +{ brake_left(); - brake_right(); + brake_right(); } -void Motors::set_right_motor_speed(float speed){ - if(speed < 0){ - hb1_r = 0; - hb2_r = 1; - right_motor_speed = -get_adjusted_speed(-speed); - enable_r = -right_motor_speed; - }else{ - hb1_r = 1; - hb2_r = 0; - right_motor_speed = get_adjusted_speed(speed); - enable_r = right_motor_speed; - } +void Motors::set_right_motor_speed(float speed) +{ + if(speed == 0) { + right_motor_speed = 0; + enable_r = 0; + } else { + if(speed < 0) { + hb1_r = 1; + hb2_r = 0; + right_motor_speed = -get_adjusted_speed(-speed); + enable_r = -right_motor_speed; + } else { + hb1_r = 0; + hb2_r = 1; + right_motor_speed = get_adjusted_speed(speed); + enable_r = right_motor_speed; + } + } } -void Motors::set_pwm_period(int period_in){ +void Motors::set_pwm_period(int period_in) +{ motor_pwm_period = period_in; enable_l.period_us(motor_pwm_period); - enable_r.period_us(motor_pwm_period); + enable_r.period_us(motor_pwm_period); } -void Motors::init(){ +void Motors::init() +{ if(motor_pwm_period == 0) motor_pwm_period = MOTOR_PWM_PERIOD_US; enable_l.period_us(motor_pwm_period); enable_r.period_us(motor_pwm_period);