Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of PsiSwarm-flockingAddedBluetooth by
Diff: PsiSwarm/motors.cpp
- Revision:
- 9:085e090e1ec1
- Parent:
- 8:00558287a4ef
- Child:
- 13:f5994956b1ba
--- a/PsiSwarm/motors.cpp Thu Oct 22 15:36:16 2015 +0000 +++ b/PsiSwarm/motors.cpp Mon Oct 26 11:16:05 2015 +0000 @@ -14,6 +14,7 @@ #include "psiswarm.h" Timeout time_based_action_timeout; +char brake_when_done = 0; void set_left_motor_speed(float speed) { @@ -89,52 +90,108 @@ IF_update_motors(); } -void time_based_turn(float speed, int microseconds) +//Forward for a set period of time +void time_based_forward(float speed, int microseconds, char brake) { + //Check if a current time based action is running - if it is, throw a warning and cancel its timeout + IF_check_time_for_existing_time_based_action(); + + //Start moving + forward(speed); + brake_when_done = brake; + time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds); +} + +//Turn for a set period of time +void time_based_turn(float speed, int microseconds, char brake) +{ + //Check if a current time based action is running - if it is, throw a warning and cancel its timeout + IF_check_time_for_existing_time_based_action(); + + //Start turning + turn(speed); + brake_when_done = brake; + time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds); } -void time_based_turn_degrees(float speed, float degrees) +//Return the time in microseconds that performing the turn will take +int get_time_based_turn_time(float speed, float degrees) +{ + //Check sign of degrees + if(degrees < 0) degrees =- degrees; + + //Main calculation for turn time + float turn_time = degrees / ((290 * speed)); + + //Add a hard offset of 4ms to account for start\stop time + if(degrees > 4) { + turn_time += 0.004; + } else turn_time += 0.002; + + // Add offset for slow speed + if(speed<0.31) { + float mul_fact = 0.31 - speed; + if(mul_fact < 0) mul_fact = 0; + mul_fact /= 2; + mul_fact += 1; + turn_time *= mul_fact; + } + + // Add offset for short turns + if(degrees < 360) { + float short_offset_multiplier = 1.0 + (0.9 / degrees); + turn_time *= short_offset_multiplier; + } + + // Convert to uS + turn_time *= 1000000; + + return (int) turn_time; +} + +//Turn the robot a set number of degrees [using time estimation to end turn] +int time_based_turn_degrees(float speed, float degrees, char brake) { if(speed < 0 || speed > 1 || degrees == 0) { - debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees); + debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees); + return 0; } else { - char invert = 0; - if(degrees < 0) {degrees=-degrees; invert = 1;} - - //Main calculation for turn time - float turn_time = degrees / ((290 * speed)); + //Check if a current time based action is running - if it is, throw a warning and cancel its timeout + IF_check_time_for_existing_time_based_action(); - //Add a hard offset of 4ms to account for start\stop time - if(degrees > 4) { - turn_time += 0.004; - } else turn_time += 0.002; + //Calculate turn time using get_time_based_turn_time + int turn_time = get_time_based_turn_time(speed,degrees); - // Add offset for slow speed - if(speed<0.31) { - float mul_fact = 0.31 - speed; - if(mul_fact < 0) mul_fact = 0; - mul_fact /= 2; - mul_fact += 1; - turn_time *= mul_fact; + //Set correct turn direction (-degrees is a counter-clockwise turn) + if(degrees < 0) { + degrees=-degrees; + speed=-speed; } - // Add offset for short turns - if(degrees < 360) { - float short_offset_multiplier = 1.0 + (0.9 / degrees); - turn_time *= short_offset_multiplier; - } + //Start turning + turn(speed); - //pc.printf("Speed: %f Turn time: %f\n",speed,turn_time); - if(invert) speed=-speed; - turn(speed); - time_based_action_timeout.attach(&IF_end_time_based_action,turn_time); -} + brake_when_done = brake; + time_based_action_timeout.attach_us(&IF_end_time_based_action,turn_time); + return turn_time; + } } +//Check if a current time based action is running - if it is, throw a warning and cancel its timeout +void IF_check_time_for_existing_time_based_action() +{ + if(time_based_motor_action == 1){ + time_based_action_timeout.detach(); + debug("WARNING: New time-based action called before previous action finished!\n"); + } + else time_based_motor_action = 1; +} void IF_end_time_based_action() { - brake(); + if(brake_when_done == 1)brake(); + else stop(); + time_based_motor_action = 0; } void IF_update_motors()