Diff: motors.cpp

Revision:

3:8762f6b2ea8d

Parent:

0:8a2dd255c508

Child:

5:6da8daaeb9f7

--- a/motors.cpp	Sat Nov 26 21:50:10 2016 +0000
+++ b/motors.cpp	Mon Nov 28 22:41:14 2016 +0000
@@ -11,6 +11,33 @@
 DigitalOut hb1_r(p24);
 DigitalOut hb2_l(p29);
 DigitalOut hb2_r(p30);
+float left_motor_speed = 0;
+float right_motor_speed = 0;
+
+float Motors::get_left_motor_speed()
+{
+    return left_motor_speed;   
+}
+
+float Motors::get_right_motor_speed()
+{
+    return right_motor_speed;   
+}
+
+void Motors::forwards(float speed){
+    set_left_motor_speed(speed);
+    set_right_motor_speed(speed);    
+}
+
+void Motors::backwards(float speed){
+    set_left_motor_speed(-speed);
+    set_right_motor_speed(-speed);   
+}
+
+void Motors::turn(float speed){
+    set_left_motor_speed(speed);
+    set_right_motor_speed(-speed);   
+}
 
 float Motors::get_current_left(){
     float current = motor_vprop_l.read();
@@ -34,6 +61,8 @@
 
 void Motors::sleep(){
     nsleep = 0;   
+    left_motor_speed = 0;
+    right_motor_speed = 0;
 }
 
 void Motors::wake_up(){
@@ -42,34 +71,40 @@
 
 void Motors::coast_left(){
     enable_l = 0;       
+    left_motor_speed = 0;
 }
 
 void Motors::brake_left(){
     hb1_l = 1;
     hb2_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;
-        enable_l = get_adjusted_speed(-speed);
+        left_motor_speed = -get_adjusted_speed(-speed);
+        enable_l = -left_motor_speed;
     }else{
         hb1_l = 1;
         hb2_l = 0;
-        enable_l = get_adjusted_speed(speed);   
+        left_motor_speed = get_adjusted_speed(speed);
+        enable_l = left_motor_speed;   
     }   
 }
 
 void Motors::coast_right(){
     enable_r = 0;       
+    right_motor_speed = 0;
 }
 
 void Motors::brake_right(){
     hb1_r = 1;
     hb2_r = 1;
     enable_r = 1;   
+    right_motor_speed = 0;
 }
 
 void Motors::coast(){
@@ -86,11 +121,13 @@
     if(speed < 0){
         hb1_r = 0;
         hb2_r = 1;
-        enable_r = get_adjusted_speed(-speed);
+        right_motor_speed = -get_adjusted_speed(-speed);
+        enable_r = -right_motor_speed;
     }else{
         hb1_r = 1;
         hb2_r = 0;
-        enable_r = get_adjusted_speed(speed);   
+        right_motor_speed = get_adjusted_speed(speed);
+        enable_r = right_motor_speed;   
     }   
 }