![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
PsiSwarm/motors.cpp
- Committer:
- jah128
- Date:
- 2015-10-27
- Revision:
- 13:f5994956b1ba
- Parent:
- 9:085e090e1ec1
File content as of revision 13:f5994956b1ba:
/* 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, Homero Elizondo, Jon Timmis * * PsiSwarm Library Version: 0.3 * * October 2015 * */ #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(); }