James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/motors.cpp@6:ff3c66f7372b, 2015-10-22 (annotated)

Committer:
jah128
Date:
Thu Oct 22 00:46:14 2015 +0000
Revision:
6:ff3c66f7372b
Parent:
0:8a5497a2e366
Child:
8:00558287a4ef
Initial version: beacon detection and sync. code, bearing estimation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:8a5497a2e366 1 /* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
jah128 0:8a5497a2e366 2 *
jah128 0:8a5497a2e366 3 * File: motors.cpp
jah128 0:8a5497a2e366 4 *
jah128 6:ff3c66f7372b 5 * (C) Dept. Electronics & Computer Science, University of York
jah128 6:ff3c66f7372b 6 * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 0:8a5497a2e366 7 *
jah128 6:ff3c66f7372b 8 * PsiSwarm Library Version: 0.3
jah128 0:8a5497a2e366 9 *
jah128 6:ff3c66f7372b 10 * October 2015
jah128 0:8a5497a2e366 11 *
jah128 0:8a5497a2e366 12 */
jah128 0:8a5497a2e366 13
jah128 0:8a5497a2e366 14 #include "psiswarm.h"
jah128 0:8a5497a2e366 15
jah128 0:8a5497a2e366 16 void set_left_motor_speed(float speed)
jah128 0:8a5497a2e366 17 {
jah128 0:8a5497a2e366 18 motor_left_speed = speed;
jah128 0:8a5497a2e366 19 motor_left_brake = 0;
jah128 0:8a5497a2e366 20 IF_update_motors();
jah128 0:8a5497a2e366 21 }
jah128 0:8a5497a2e366 22
jah128 0:8a5497a2e366 23 void set_right_motor_speed(float speed)
jah128 0:8a5497a2e366 24 {
jah128 0:8a5497a2e366 25 motor_right_speed = speed;
jah128 0:8a5497a2e366 26 motor_right_brake = 0;
jah128 0:8a5497a2e366 27 IF_update_motors();
jah128 0:8a5497a2e366 28 }
jah128 0:8a5497a2e366 29
jah128 0:8a5497a2e366 30 void brake_left_motor()
jah128 0:8a5497a2e366 31 {
jah128 0:8a5497a2e366 32 motor_left_speed = 0;
jah128 0:8a5497a2e366 33 motor_left_brake = 1;
jah128 0:8a5497a2e366 34 IF_update_motors();
jah128 0:8a5497a2e366 35 }
jah128 0:8a5497a2e366 36
jah128 0:8a5497a2e366 37 void brake_right_motor()
jah128 0:8a5497a2e366 38 {
jah128 0:8a5497a2e366 39 motor_right_speed = 0;
jah128 0:8a5497a2e366 40 motor_right_brake = 1;
jah128 0:8a5497a2e366 41 IF_update_motors();
jah128 0:8a5497a2e366 42 }
jah128 0:8a5497a2e366 43
jah128 0:8a5497a2e366 44 void brake()
jah128 0:8a5497a2e366 45 {
jah128 0:8a5497a2e366 46 motor_left_speed = 0;
jah128 0:8a5497a2e366 47 motor_right_speed = 0;
jah128 0:8a5497a2e366 48 motor_left_brake = 1;
jah128 0:8a5497a2e366 49 motor_right_brake = 1;
jah128 0:8a5497a2e366 50 IF_update_motors();
jah128 0:8a5497a2e366 51 }
jah128 0:8a5497a2e366 52
jah128 0:8a5497a2e366 53 void stop()
jah128 0:8a5497a2e366 54 {
jah128 0:8a5497a2e366 55 motor_left_speed = 0;
jah128 0:8a5497a2e366 56 motor_right_speed = 0;
jah128 0:8a5497a2e366 57 motor_left_brake = 0;
jah128 0:8a5497a2e366 58 motor_right_brake = 0;
jah128 0:8a5497a2e366 59 IF_update_motors();
jah128 0:8a5497a2e366 60 }
jah128 0:8a5497a2e366 61
jah128 0:8a5497a2e366 62 void forward(float speed)
jah128 0:8a5497a2e366 63 {
jah128 0:8a5497a2e366 64 motor_left_speed = speed;
jah128 0:8a5497a2e366 65 motor_right_speed = speed;
jah128 0:8a5497a2e366 66 motor_left_brake = 0;
jah128 0:8a5497a2e366 67 motor_right_brake = 0;
jah128 0:8a5497a2e366 68 IF_update_motors();
jah128 0:8a5497a2e366 69 }
jah128 0:8a5497a2e366 70
jah128 0:8a5497a2e366 71 void backward(float speed)
jah128 0:8a5497a2e366 72 {
jah128 0:8a5497a2e366 73 motor_left_speed = -speed;
jah128 0:8a5497a2e366 74 motor_right_speed = -speed;
jah128 0:8a5497a2e366 75 motor_left_brake = 0;
jah128 0:8a5497a2e366 76 motor_right_brake = 0;
jah128 0:8a5497a2e366 77 IF_update_motors();
jah128 0:8a5497a2e366 78 }
jah128 0:8a5497a2e366 79
jah128 0:8a5497a2e366 80 void turn(float speed)
jah128 0:8a5497a2e366 81 {
jah128 0:8a5497a2e366 82 //A positive turn is clockwise
jah128 0:8a5497a2e366 83 motor_left_speed = speed;
jah128 0:8a5497a2e366 84 motor_right_speed = -speed;
jah128 0:8a5497a2e366 85 motor_left_brake = 0;
jah128 0:8a5497a2e366 86 motor_right_brake = 0;
jah128 0:8a5497a2e366 87 IF_update_motors();
jah128 0:8a5497a2e366 88 }
jah128 0:8a5497a2e366 89
jah128 0:8a5497a2e366 90 void IF_update_motors(){
jah128 0:8a5497a2e366 91 if(motor_left_speed > 1.0){
jah128 0:8a5497a2e366 92 motor_left_speed = 1.0;
jah128 0:8a5497a2e366 93 //Throw exception...
jah128 0:8a5497a2e366 94 }
jah128 0:8a5497a2e366 95 if(motor_right_speed > 1.0){
jah128 0:8a5497a2e366 96 motor_right_speed = 1.0;
jah128 0:8a5497a2e366 97 //Throw exception...
jah128 0:8a5497a2e366 98 }
jah128 0:8a5497a2e366 99 if(motor_left_speed < -1.0){
jah128 0:8a5497a2e366 100 motor_left_speed = -1.0;
jah128 0:8a5497a2e366 101 //Throw exception...
jah128 0:8a5497a2e366 102 }
jah128 0:8a5497a2e366 103 if(motor_right_speed < -1.0){
jah128 0:8a5497a2e366 104 motor_right_speed = -1.0;
jah128 0:8a5497a2e366 105 //Throw exception...
jah128 0:8a5497a2e366 106 }
jah128 0:8a5497a2e366 107 if(motor_left_brake){
jah128 0:8a5497a2e366 108 motor_left_f.write(1);
jah128 0:8a5497a2e366 109 motor_left_r.write(1);
jah128 0:8a5497a2e366 110 if(motor_left_speed!=0){
jah128 0:8a5497a2e366 111 motor_left_speed = 0;
jah128 0:8a5497a2e366 112 //Throw exception...
jah128 0:8a5497a2e366 113 }
jah128 0:8a5497a2e366 114 }else{
jah128 0:8a5497a2e366 115 if(motor_left_speed >= 0){
jah128 0:8a5497a2e366 116 motor_left_f.write(0);
jah128 0:8a5497a2e366 117 motor_left_r.write(IF_calibrated_speed(motor_left_speed));
jah128 0:8a5497a2e366 118
jah128 0:8a5497a2e366 119 }else{
jah128 0:8a5497a2e366 120 motor_left_r.write(0);
jah128 0:8a5497a2e366 121 motor_left_f.write(IF_calibrated_speed(-motor_left_speed));
jah128 0:8a5497a2e366 122 }
jah128 0:8a5497a2e366 123 }
jah128 0:8a5497a2e366 124 if(motor_right_brake){
jah128 0:8a5497a2e366 125 motor_right_f.write(1);
jah128 0:8a5497a2e366 126 motor_right_r.write(1);
jah128 0:8a5497a2e366 127 if(motor_right_speed!=0){
jah128 0:8a5497a2e366 128 motor_right_speed = 0;
jah128 0:8a5497a2e366 129 //Throw exception...
jah128 0:8a5497a2e366 130 }
jah128 0:8a5497a2e366 131 }else{
jah128 0:8a5497a2e366 132 if(motor_right_speed >= 0){
jah128 0:8a5497a2e366 133 motor_right_f.write(0);
jah128 0:8a5497a2e366 134 motor_right_r.write(IF_calibrated_speed(motor_right_speed));
jah128 0:8a5497a2e366 135 }else{
jah128 0:8a5497a2e366 136 motor_right_r.write(0);
jah128 0:8a5497a2e366 137 motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
jah128 0:8a5497a2e366 138 }
jah128 0:8a5497a2e366 139 }
jah128 0:8a5497a2e366 140
jah128 0:8a5497a2e366 141 }
jah128 0:8a5497a2e366 142
jah128 0:8a5497a2e366 143
jah128 0:8a5497a2e366 144 float IF_calibrated_speed(float speed){
jah128 0:8a5497a2e366 145 if(speed == 0) return 0;
jah128 0:8a5497a2e366 146 //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed
jah128 0:8a5497a2e366 147 float adjusted = speed;
jah128 0:8a5497a2e366 148 if(OFFSET_MOTORS) {adjusted*=0.8f; adjusted+=0.2;}
jah128 0:8a5497a2e366 149 return adjusted;
jah128 0:8a5497a2e366 150 }
jah128 0:8a5497a2e366 151
jah128 0:8a5497a2e366 152 void IF_init_motors(){
jah128 0:8a5497a2e366 153 // Motor PWM outputs work optimally at 25kHz frequency
jah128 0:8a5497a2e366 154 motor_left_f.period_us(40);
jah128 0:8a5497a2e366 155 motor_right_f.period_us(40);
jah128 0:8a5497a2e366 156 motor_left_r.period_us(40);
jah128 0:8a5497a2e366 157 motor_right_r.period_us(40);
jah128 0:8a5497a2e366 158 motor_left_speed = 0;
jah128 0:8a5497a2e366 159 motor_right_speed = 0;
jah128 0:8a5497a2e366 160 motor_left_brake = 0;
jah128 0:8a5497a2e366 161 motor_right_brake = 0;
jah128 0:8a5497a2e366 162 IF_update_motors();
jah128 0:8a5497a2e366 163 }