Bluetooth communication for flocking.
Fork of BeautifulMemeProject 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()