Diff: motors.cpp

Revision:

18:9204f74069b4

Parent:

16:50686c07ad07

--- a/motors.cpp	Sun Jun 04 13:11:09 2017 +0000
+++ b/motors.cpp	Sun Jun 04 20:22:41 2017 +0000
@@ -266,24 +266,40 @@
 
 float Motors::IF_calibrated_left_speed(float speed)
 {
+    if(speed==0) return 0;
     //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
-    float adjusted  = IF_calibrated_speed(speed);
-    if(use_motor_calibration) {
+    float adjusted  = speed;
+    if(use_motor_calibration == 1) {
         adjusted *= left_motor_calibration_value;
     }
+    if(OFFSET_MOTORS == 1 || use_motor_calibration == 1){
+        adjusted*=(1.0-left_motor_stall_offset);
+        adjusted+=left_motor_stall_offset;   
+    }
+    if(adjusted<0) return 0;
+    if(adjusted>1) return 1;
     return adjusted;
 }
 
 float Motors::IF_calibrated_right_speed(float speed)
 {
+    if(speed==0) return 0;
     //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
-    float adjusted  = IF_calibrated_speed(speed);
-    if(use_motor_calibration) {
+    float adjusted = speed;
+    if(use_motor_calibration == 1) {
         adjusted *= right_motor_calibration_value;
     }
+    if(OFFSET_MOTORS == 1 || use_motor_calibration == 1){
+        adjusted*=(1.0-right_motor_stall_offset);
+        adjusted+=right_motor_stall_offset;   
+    }
+    
+    if(adjusted<0) return 0;
+    if(adjusted>1) return 1;
     return adjusted;
 }
 
+/* Remove from version 0.9 as each motor has unique offset
 float Motors::IF_calibrated_speed(float speed)
 {
     if(speed == 0) return 0;
@@ -295,6 +311,102 @@
     }
     return adjusted;
 }
+*/
+
+
+void Motors::calibrate_motors (void)
+{
+    char restore_use_motor_calibration = use_motor_calibration;
+    //Turn off motor calibration whilst testing values!
+    use_motor_calibration = 0;
+    
+
+    pc.printf("\nMotor Calibration Routine Started\n");
+    display.clear_display();
+    display.write_string("Starting motor");
+    display.set_position(1,0);
+    display.write_string("calibration...");
+    wait(1);
+    char left_offset = 0;
+    char right_offset = 0;
+    float left_calibration_value = 1;
+    float right_calibration_value = 1;
+    for(int i=0;i<2;i++){
+        display.clear_display();
+        if(i==0)display.write_string("LEFT MOTOR");
+        else display.write_string("RIGHT MOTOR");
+        display.set_position(1,0);
+        display.write_string("STALL SPEED");
+        wait(1);
+        wait(1);
+        while(i2c_setup.IF_get_switch_state() != 0) {
+          display.clear_display();
+          display.set_position(1,0);
+          display.write_string("RELEASE SWITCH!");
+          wait(0.1);
+        }
+        display.set_position(1,0);
+        display.write_string("                ");
+        char a_msg [17];
+        char cutoff = 0;
+        for(int j=5;j<40;j++){
+            display.set_position(1,0);
+            sprintf(a_msg,"%d ",j);
+            display.write_string(a_msg);
+            float target_speed = j * 0.01;
+            if(i==0) set_left_motor_speed(target_speed);
+            else set_right_motor_speed(target_speed);
+            char s_count = 0;
+            char switch_state = i2c_setup.IF_get_switch_state();
+            while(switch_state == 0 && s_count < 10) {
+              s_count ++;
+              switch_state = i2c_setup.IF_get_switch_state();
+              wait(0.1);
+            }
+            if(s_count < 10){
+                cutoff = j; 
+                break;
+            }
+            motors.brake();
+            wait(0.05);
+        }
+        if (cutoff > 0){
+            if(i==0) left_offset = cutoff;
+            else right_offset = cutoff;
+            pc.printf("Motor stall speed indicated as %f\n",(cutoff * 0.01f));   
+        }else pc.printf("Error: Button not pressed\n");
+        
+    }
+
+
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string("^ REJECT");
+    display.set_position(1,2);
+    display.write_string("ACCEPT");
+    char switch_pos = i2c_setup.IF_get_switch_state();
+    while(switch_pos != 1 && switch_pos != 2) switch_pos = i2c_setup.IF_get_switch_state();
+    if(switch_pos == 2) {
+        pc.printf("\nChanges accepted:  Updating firmware values in EEPROM\n");
+        display.clear_display();
+        display.set_position(0,0);
+        display.write_string("UPDATING");
+        display.set_position(1,0);
+        display.write_string("FIRMWARE");
+        wait(0.5);
+        
+        eprom.IF_write_motor_calibration_values(left_calibration_value, left_offset, right_calibration_value, right_offset);
+        wait(1.5);
+    } else {
+        pc.printf("\nChanges rejected.\n");
+    }
+    display.clear_display();
+    wait(0.5);
+    pc.printf("Motor calibration routine complete\n\n");
+    //Restore motor calibration parameters
+    use_motor_calibration = restore_use_motor_calibration;
+}
+
 
 void Motors::init_motors()
 {