Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
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?
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 | 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 | } |