Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
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?
User | Revision | Line number | New 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 | } |