James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/motors.cpp@0:8a5497a2e366, 2015-10-03 (annotated)

Committer:
jah128
Date:
Sat Oct 03 22:48:50 2015 +0000
Revision:
0:8a5497a2e366
Child:
6:ff3c66f7372b
Initial commit of PsiSwarm API and example code

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