Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
PsiSwarm/motors.cpp
- Committer:
- jah128
- Date:
- 2015-10-03
- Revision:
- 0:8a5497a2e366
- Child:
- 6:ff3c66f7372b
File content as of revision 0:8a5497a2e366:
/* 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(); }