Revision 6:732aa91eb555, committed 2017-01-13

Comitter:

jah128

Date:

Fri Jan 13 23:16:23 2017 +0000

Parent:

5:6da8daaeb9f7

Commit message:

Updated;

Changed in this revision

--- 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);