Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
PsiSwarm/motors.cpp
- Committer:
- jah128
- Date:
- 2015-10-22
- Revision:
- 8:00558287a4ef
- Parent:
- 6:ff3c66f7372b
- Child:
- 9:085e090e1ec1
File content as of revision 8:00558287a4ef:
/* 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; 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(); } void time_based_turn(float speed, int microseconds) { } 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... } 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(); }