Diff: motors.cpp

Revision:

0:8a2dd255c508

Child:

3:8762f6b2ea8d

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