Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
Diff: PsiSwarm/motors.cpp
- Revision:
- 8:00558287a4ef
- Parent:
- 6:ff3c66f7372b
- Child:
- 9:085e090e1ec1
--- a/PsiSwarm/motors.cpp Thu Oct 22 13:28:17 2015 +0000 +++ b/PsiSwarm/motors.cpp Thu Oct 22 15:36:16 2015 +0000 @@ -1,18 +1,20 @@ /* 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; + void set_left_motor_speed(float speed) { motor_left_speed = speed; @@ -84,80 +86,134 @@ motor_right_speed = -speed; motor_left_brake = 0; motor_right_brake = 0; - IF_update_motors(); + IF_update_motors(); +} + +void time_based_turn(float speed, int microseconds) +{ } -void IF_update_motors(){ - if(motor_left_speed > 1.0){ +void time_based_turn_degrees(float speed, float degrees) +{ + if(speed < 0 || speed > 1 || degrees == 0) { + debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees); + } else { + char invert = 0; + if(degrees < 0) {degrees=-degrees; invert = 1;} + + //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; + } + + //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); +} +} + + +void IF_end_time_based_action() +{ + brake(); +} + +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... + //Throw exception... } - if(motor_right_speed < -1.0){ - motor_right_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_left_brake){ - motor_left_f.write(1); - motor_left_r.write(1); - if(motor_left_speed!=0){ - motor_left_speed = 0; + if(motor_right_speed < -1.0) { + motor_right_speed = -1.0; //Throw exception... - } - }else{ - if(motor_left_speed >= 0){ + } + 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{ + + } 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){ + 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{ + } else { motor_right_r.write(0); motor_right_f.write(IF_calibrated_speed(-motor_right_speed)); } } - + } -float IF_calibrated_speed(float 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; + if(OFFSET_MOTORS) { + adjusted*=0.8f; + adjusted+=0.2; + } + return adjusted; } -void IF_init_motors(){ +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_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(); + motor_right_brake = 0; + IF_update_motors(); } \ No newline at end of file