Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
Diff: PsiSwarm/motors.cpp
- Revision:
- 0:8a5497a2e366
- Child:
- 6:ff3c66f7372b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PsiSwarm/motors.cpp Sat Oct 03 22:48:50 2015 +0000 @@ -0,0 +1,162 @@ +/* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File + * + * File: motors.cpp + * + * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York + * + * PsiSwarm Library Version: 0.2 + * + * September 2015 + * + */ + +#include "psiswarm.h" + +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 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