Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
Diff: motors.cpp
- Revision:
- 18:9204f74069b4
- Parent:
- 16:50686c07ad07
- Child:
- 20:2b6ebe60929d
diff -r bf614e28668f -r 9204f74069b4 motors.cpp --- 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() {