Practical Robotics Modular Robot Library
Diff: motors.cpp
- Revision:
- 0:8a2dd255c508
- Child:
- 3:8762f6b2ea8d
diff -r 000000000000 -r 8a2dd255c508 motors.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motors.cpp Sat Nov 26 17:28:53 2016 +0000 @@ -0,0 +1,104 @@ +#include "robot.h" + +DigitalOut nsleep(p7); +DigitalIn nfault_l(p8,PullUp); +DigitalIn nfault_r(p11,PullUp); +AnalogIn motor_vprop_l(p19); +AnalogIn motor_vprop_r(p20); +PwmOut enable_l(p21); +PwmOut enable_r(p22); +DigitalOut hb1_l(p23); +DigitalOut hb1_r(p24); +DigitalOut hb2_l(p29); +DigitalOut hb2_r(p30); + +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; +} + +float Motors::get_current_right(){ + float current = motor_vprop_r.read(); + return current * 2.64f; +} + +float Motors::get_adjusted_speed(float speed_in){ + float ret_speed = speed_in; + if(USE_STALL_OFFSET == 1){ + ret_speed += STALL_OFFSET; + ret_speed /= (STALL_OFFSET + 1.0f); + } + return ret_speed; +} + +void Motors::sleep(){ + nsleep = 0; +} + +void Motors::wake_up(){ + nsleep = 1; +} + +void Motors::coast_left(){ + enable_l = 0; +} + +void Motors::brake_left(){ + hb1_l = 1; + hb2_l = 1; + enable_l = 1; +} + +void Motors::set_left_motor_speed(float speed){ + if(speed < 0){ + hb1_l = 0; + hb2_l = 1; + enable_l = get_adjusted_speed(-speed); + }else{ + hb1_l = 1; + hb2_l = 0; + enable_l = get_adjusted_speed(speed); + } +} + +void Motors::coast_right(){ + enable_r = 0; +} + +void Motors::brake_right(){ + hb1_r = 1; + hb2_r = 1; + enable_r = 1; +} + +void Motors::coast(){ + coast_left(); + coast_right(); +} + +void Motors::brake(){ + brake_left(); + brake_right(); +} + +void Motors::set_right_motor_speed(float speed){ + if(speed < 0){ + hb1_r = 0; + hb2_r = 1; + enable_r = get_adjusted_speed(-speed); + }else{ + hb1_r = 1; + hb2_r = 0; + enable_r = get_adjusted_speed(speed); + } +} + + +void Motors::init(){ + enable_l.period_us(MOTOR_PWM_PERIOD_US); + enable_r.period_us(MOTOR_PWM_PERIOD_US); + enable_l = 0; + enable_r = 0; + wake_up(); +} \ No newline at end of file