Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
Diff: motors.cpp
- Revision:
- 0:d6269d17c8cf
- Child:
- 2:c6986ee3c7c5
diff -r 000000000000 -r d6269d17c8cf motors.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motors.cpp Thu Feb 04 21:48:54 2016 +0000 @@ -0,0 +1,283 @@ +/* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File + * + * File: motors.cpp + * + * (C) Dept. Electronics & Computer Science, University of York + * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis + * + * PsiSwarm Library Version: 0.4 + * + * February 2016 + * + * + */ + +#include "psiswarm.h" + +Timeout time_based_action_timeout; +char brake_when_done = 0; + +void set_left_motor_speed(float speed) +{ + motor_left_speed = speed; + motor_left_brake = 0; + IF_update_motors(); +} + +void set_right_motor_speed(float speed) +{ + motor_right_speed = speed; + motor_right_brake = 0; + IF_update_motors(); +} + +void brake_left_motor() +{ + motor_left_speed = 0; + motor_left_brake = 1; + IF_update_motors(); +} + +void brake_right_motor() +{ + motor_right_speed = 0; + motor_right_brake = 1; + IF_update_motors(); +} + +void brake() +{ + motor_left_speed = 0; + motor_right_speed = 0; + motor_left_brake = 1; + motor_right_brake = 1; + IF_update_motors(); +} + +void stop() +{ + motor_left_speed = 0; + motor_right_speed = 0; + motor_left_brake = 0; + motor_right_brake = 0; + IF_update_motors(); +} + +void forward(float speed) +{ + motor_left_speed = speed; + motor_right_speed = speed; + motor_left_brake = 0; + motor_right_brake = 0; + IF_update_motors(); +} + +void backward(float speed) +{ + motor_left_speed = -speed; + motor_right_speed = -speed; + motor_left_brake = 0; + motor_right_brake = 0; + IF_update_motors(); +} + +void turn(float speed) +{ + //A positive turn is clockwise + motor_left_speed = speed; + motor_right_speed = -speed; + motor_left_brake = 0; + motor_right_brake = 0; + IF_update_motors(); +} + +//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); +} + +//Returns the limit of degrees available to turn in given time +float get_maximum_turn_angle(int microseconds){ + //We can turn at about 270 degrees per second at full speed + return (microseconds * 0.00027); +} + +//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); + return 0; + } else { + //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(); + + //Calculate turn time using get_time_based_turn_time + int turn_time = get_time_based_turn_time(speed,degrees); + + //Set correct turn direction (-degrees is a counter-clockwise turn) + if(degrees < 0) { + degrees=-degrees; + speed=-speed; + } + + //Start turning + turn(speed); + + 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() +{ + if(brake_when_done == 1)brake(); + else stop(); + time_based_motor_action = 0; +} + +void IF_update_motors() +{ + if(motor_left_speed > 1.0) { + motor_left_speed = 1.0; + //Throw exception... + } + if(motor_right_speed > 1.0) { + motor_right_speed = 1.0; + //Throw exception... + } + if(motor_left_speed < -1.0) { + motor_left_speed = -1.0; + //Throw exception... + } + if(motor_right_speed < -1.0) { + motor_right_speed = -1.0; + //Throw exception... + } + if(motor_left_brake) { + motor_left_f.write(1); + motor_left_r.write(1); + if(motor_left_speed!=0) { + motor_left_speed = 0; + //Throw exception... + } + } else { + if(motor_left_speed >= 0) { + motor_left_f.write(0); + motor_left_r.write(IF_calibrated_speed(motor_left_speed)); + + } else { + motor_left_r.write(0); + motor_left_f.write(IF_calibrated_speed(-motor_left_speed)); + } + } + if(motor_right_brake) { + motor_right_f.write(1); + motor_right_r.write(1); + if(motor_right_speed!=0) { + motor_right_speed = 0; + //Throw exception... + } + } else { + if(motor_right_speed >= 0) { + motor_right_f.write(0); + motor_right_r.write(IF_calibrated_speed(motor_right_speed)); + } else { + motor_right_r.write(0); + motor_right_f.write(IF_calibrated_speed(-motor_right_speed)); + } + } + +} + + +float IF_calibrated_speed(float speed) +{ + if(speed == 0) return 0; + //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed + float adjusted = speed; + if(OFFSET_MOTORS) { + adjusted*=0.8f; + adjusted+=0.2; + } + return adjusted; +} + +void IF_init_motors() +{ + // Motor PWM outputs work optimally at 25kHz frequency + motor_left_f.period_us(40); + motor_right_f.period_us(40); + motor_left_r.period_us(40); + motor_right_r.period_us(40); + motor_left_speed = 0; + motor_right_speed = 0; + motor_left_brake = 0; + motor_right_brake = 0; + IF_update_motors(); +} \ No newline at end of file