Mouse code for the MacroRat
main.cpp@17:f713758f6238, 2017-05-14 (annotated)
- Committer:
- sahilmgandhi
- Date:
- Sun May 14 23:07:23 2017 +0000
- Revision:
- 17:f713758f6238
- Parent:
- 16:d9252437bd92
- Child:
- 18:6a4db94011d3
Added mbed-dev and still working on dynamically deciding to turn;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sahilmgandhi | 0:a03c771ab78e | 1 | #include "mbed.h" |
kyleliangus | 4:b5b7836ca2b0 | 2 | |
kyleliangus | 6:3d68fedd6fd9 | 3 | #include "irpair.h" |
kyleliangus | 4:b5b7836ca2b0 | 4 | #include "main.h" |
kyleliangus | 4:b5b7836ca2b0 | 5 | #include "motor.h" |
kyleliangus | 4:b5b7836ca2b0 | 6 | |
sahilmgandhi | 0:a03c771ab78e | 7 | #include <stdlib.h> |
sahilmgandhi | 1:8a4b2f573923 | 8 | #include "ITG3200.h" |
sahilmgandhi | 14:9e7bb03ddccb | 9 | #include "stm32f4xx.h" |
sahilmgandhi | 7:6f5cb6377bd4 | 10 | #include "QEI.h" |
sahilmgandhi | 0:a03c771ab78e | 11 | |
vanshg | 11:8fc2b703086b | 12 | /* Constants for when HIGH_PWM_VOLTAGE = 0.2 |
kyleliangus | 9:1d8e4da058cd | 13 | #define IP_CONSTANT 6 |
kyleliangus | 9:1d8e4da058cd | 14 | #define II_CONSTANT 0 |
kyleliangus | 9:1d8e4da058cd | 15 | #define ID_CONSTANT 1 |
vanshg | 11:8fc2b703086b | 16 | */ |
sahilmgandhi | 0:a03c771ab78e | 17 | |
vanshg | 11:8fc2b703086b | 18 | // Constants for when HIGH_PWM_VOLTAGE = 0.1 |
kyleliangus | 15:b80555a4a8b9 | 19 | #define IP_CONSTANT 9 |
kyleliangus | 15:b80555a4a8b9 | 20 | #define II_CONSTANT 1 |
kyleliangus | 15:b80555a4a8b9 | 21 | #define ID_CONSTANT 3 |
sahilmgandhi | 14:9e7bb03ddccb | 22 | |
sahilmgandhi | 14:9e7bb03ddccb | 23 | const int desiredCountR = 1400; |
sahilmgandhi | 14:9e7bb03ddccb | 24 | const int desiredCountL = 1475; |
christine222 | 13:2032db00f168 | 25 | |
sahilmgandhi | 16:d9252437bd92 | 26 | const int oneCellCount = 5400; |
sahilmgandhi | 16:d9252437bd92 | 27 | const int oneCellCountMomentum = 4600; // one cell count is actually approximately 5400, but this value is considering momentum! |
sahilmgandhi | 16:d9252437bd92 | 28 | |
sahilmgandhi | 17:f713758f6238 | 29 | float receiverOneReading = 0.0; |
sahilmgandhi | 17:f713758f6238 | 30 | float receiverTwoReading = 0.0; |
sahilmgandhi | 17:f713758f6238 | 31 | float receiverThreeReading = 0.0; |
sahilmgandhi | 17:f713758f6238 | 32 | float receiverFourReading = 0.0; |
sahilmgandhi | 17:f713758f6238 | 33 | |
sahilmgandhi | 16:d9252437bd92 | 34 | void pidOnEncoders(); |
christine222 | 13:2032db00f168 | 35 | |
sahilmgandhi | 14:9e7bb03ddccb | 36 | void turnLeft() |
sahilmgandhi | 14:9e7bb03ddccb | 37 | { |
christine222 | 13:2032db00f168 | 38 | double speed0 = 0.15; |
sahilmgandhi | 14:9e7bb03ddccb | 39 | double speed1 = -0.15; |
christine222 | 13:2032db00f168 | 40 | |
christine222 | 13:2032db00f168 | 41 | int counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 42 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 43 | int initial1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 44 | |
sahilmgandhi | 14:9e7bb03ddccb | 45 | int desiredCount0 = initial0 - desiredCountL; |
sahilmgandhi | 14:9e7bb03ddccb | 46 | int desiredCount1 = initial1 + desiredCountL; |
christine222 | 13:2032db00f168 | 47 | |
sahilmgandhi | 14:9e7bb03ddccb | 48 | int count0 = initial0; |
sahilmgandhi | 14:9e7bb03ddccb | 49 | int count1 = initial1; |
christine222 | 13:2032db00f168 | 50 | |
christine222 | 13:2032db00f168 | 51 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 52 | double error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 53 | |
christine222 | 13:2032db00f168 | 54 | |
sahilmgandhi | 14:9e7bb03ddccb | 55 | while(1) { |
sahilmgandhi | 14:9e7bb03ddccb | 56 | |
sahilmgandhi | 14:9e7bb03ddccb | 57 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
christine222 | 13:2032db00f168 | 58 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 59 | count1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 60 | |
sahilmgandhi | 14:9e7bb03ddccb | 61 | error0 = count0 - desiredCount0; |
sahilmgandhi | 14:9e7bb03ddccb | 62 | error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 63 | |
christine222 | 13:2032db00f168 | 64 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 65 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 66 | counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 67 | } else { |
christine222 | 13:2032db00f168 | 68 | counter++; |
christine222 | 13:2032db00f168 | 69 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 70 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 71 | } |
christine222 | 13:2032db00f168 | 72 | |
sahilmgandhi | 14:9e7bb03ddccb | 73 | if (counter > 60) { |
christine222 | 13:2032db00f168 | 74 | break; |
christine222 | 13:2032db00f168 | 75 | } |
sahilmgandhi | 14:9e7bb03ddccb | 76 | } |
christine222 | 13:2032db00f168 | 77 | |
christine222 | 13:2032db00f168 | 78 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 79 | left_motor.brake(); |
sahilmgandhi | 17:f713758f6238 | 80 | turnFlag = 0; // zeroing out the flags! |
christine222 | 13:2032db00f168 | 81 | } |
kyleliangus | 9:1d8e4da058cd | 82 | |
sahilmgandhi | 14:9e7bb03ddccb | 83 | void turnRight() |
sahilmgandhi | 14:9e7bb03ddccb | 84 | { |
sahilmgandhi | 14:9e7bb03ddccb | 85 | double speed0 = -0.15; |
kyleliangus | 12:5790e56a056f | 86 | double speed1 = 0.15; |
christine222 | 13:2032db00f168 | 87 | |
christine222 | 13:2032db00f168 | 88 | int counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 89 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 90 | int initial1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 91 | |
sahilmgandhi | 14:9e7bb03ddccb | 92 | int desiredCount0 = initial0 + desiredCountR; |
sahilmgandhi | 14:9e7bb03ddccb | 93 | int desiredCount1 = initial1 - desiredCountR; |
christine222 | 13:2032db00f168 | 94 | |
sahilmgandhi | 14:9e7bb03ddccb | 95 | int count0 = initial0; |
sahilmgandhi | 14:9e7bb03ddccb | 96 | int count1 = initial1; |
kyleliangus | 12:5790e56a056f | 97 | |
christine222 | 13:2032db00f168 | 98 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 99 | double error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 100 | |
christine222 | 13:2032db00f168 | 101 | |
sahilmgandhi | 14:9e7bb03ddccb | 102 | while(1) { |
sahilmgandhi | 14:9e7bb03ddccb | 103 | |
sahilmgandhi | 14:9e7bb03ddccb | 104 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
christine222 | 13:2032db00f168 | 105 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 106 | count1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 107 | |
sahilmgandhi | 14:9e7bb03ddccb | 108 | error0 = count0 - desiredCount0; |
sahilmgandhi | 14:9e7bb03ddccb | 109 | error1 = count1 - desiredCount1; |
kyleliangus | 12:5790e56a056f | 110 | |
christine222 | 13:2032db00f168 | 111 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 112 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 113 | counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 114 | } else { |
christine222 | 13:2032db00f168 | 115 | counter++; |
christine222 | 13:2032db00f168 | 116 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 117 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 118 | } |
kyleliangus | 12:5790e56a056f | 119 | |
sahilmgandhi | 14:9e7bb03ddccb | 120 | if (counter > 60) { |
christine222 | 13:2032db00f168 | 121 | break; |
christine222 | 13:2032db00f168 | 122 | } |
christine222 | 13:2032db00f168 | 123 | } |
christine222 | 13:2032db00f168 | 124 | |
christine222 | 13:2032db00f168 | 125 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 126 | left_motor.brake(); |
sahilmgandhi | 17:f713758f6238 | 127 | turnFlag = 0; |
christine222 | 13:2032db00f168 | 128 | } |
christine222 | 13:2032db00f168 | 129 | |
sahilmgandhi | 14:9e7bb03ddccb | 130 | void turnLeft180() |
sahilmgandhi | 14:9e7bb03ddccb | 131 | { |
christine222 | 13:2032db00f168 | 132 | double speed0 = 0.15; |
sahilmgandhi | 14:9e7bb03ddccb | 133 | double speed1 = -0.15; |
christine222 | 13:2032db00f168 | 134 | |
christine222 | 13:2032db00f168 | 135 | int counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 136 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 137 | int initial1 = encoder1.getPulses(); |
kyleliangus | 12:5790e56a056f | 138 | |
sahilmgandhi | 14:9e7bb03ddccb | 139 | int desiredCount0 = initial0 - desiredCountL*2; |
sahilmgandhi | 14:9e7bb03ddccb | 140 | int desiredCount1 = initial1 + desiredCountL*2; |
christine222 | 13:2032db00f168 | 141 | |
sahilmgandhi | 14:9e7bb03ddccb | 142 | int count0 = initial0; |
sahilmgandhi | 14:9e7bb03ddccb | 143 | int count1 = initial1; |
christine222 | 13:2032db00f168 | 144 | |
christine222 | 13:2032db00f168 | 145 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 146 | double error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 147 | |
christine222 | 13:2032db00f168 | 148 | |
sahilmgandhi | 14:9e7bb03ddccb | 149 | while(1) { |
kyleliangus | 15:b80555a4a8b9 | 150 | |
sahilmgandhi | 14:9e7bb03ddccb | 151 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
christine222 | 13:2032db00f168 | 152 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 153 | count1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 154 | |
sahilmgandhi | 14:9e7bb03ddccb | 155 | error0 = count0 - desiredCount0; |
sahilmgandhi | 14:9e7bb03ddccb | 156 | error1 = count1 - desiredCount1; |
kyleliangus | 12:5790e56a056f | 157 | |
christine222 | 13:2032db00f168 | 158 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 159 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 160 | counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 161 | } else { |
christine222 | 13:2032db00f168 | 162 | counter++; |
christine222 | 13:2032db00f168 | 163 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 164 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 165 | } |
christine222 | 13:2032db00f168 | 166 | |
sahilmgandhi | 14:9e7bb03ddccb | 167 | if (counter > 60) { |
christine222 | 13:2032db00f168 | 168 | break; |
christine222 | 13:2032db00f168 | 169 | } |
kyleliangus | 12:5790e56a056f | 170 | } |
kyleliangus | 12:5790e56a056f | 171 | |
kyleliangus | 12:5790e56a056f | 172 | right_motor.brake(); |
kyleliangus | 12:5790e56a056f | 173 | left_motor.brake(); |
kyleliangus | 12:5790e56a056f | 174 | } |
kyleliangus | 12:5790e56a056f | 175 | |
sahilmgandhi | 14:9e7bb03ddccb | 176 | void turnRight180() |
sahilmgandhi | 14:9e7bb03ddccb | 177 | { |
sahilmgandhi | 14:9e7bb03ddccb | 178 | double speed0 = -0.15; |
sahilmgandhi | 14:9e7bb03ddccb | 179 | double speed1 = 0.15; |
sahilmgandhi | 14:9e7bb03ddccb | 180 | |
sahilmgandhi | 14:9e7bb03ddccb | 181 | int counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 182 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 183 | int initial1 = encoder1.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 184 | |
sahilmgandhi | 14:9e7bb03ddccb | 185 | int desiredCount0 = initial0 + desiredCountR*2; |
sahilmgandhi | 14:9e7bb03ddccb | 186 | int desiredCount1 = initial1 - desiredCountR*2; |
sahilmgandhi | 14:9e7bb03ddccb | 187 | |
sahilmgandhi | 14:9e7bb03ddccb | 188 | int count0 = initial0; |
sahilmgandhi | 14:9e7bb03ddccb | 189 | int count1 = initial1; |
sahilmgandhi | 14:9e7bb03ddccb | 190 | |
sahilmgandhi | 14:9e7bb03ddccb | 191 | double error0 = count0 - desiredCount0; |
sahilmgandhi | 14:9e7bb03ddccb | 192 | double error1 = count1 - desiredCount1; |
sahilmgandhi | 14:9e7bb03ddccb | 193 | |
sahilmgandhi | 14:9e7bb03ddccb | 194 | |
sahilmgandhi | 14:9e7bb03ddccb | 195 | while(1) { |
sahilmgandhi | 14:9e7bb03ddccb | 196 | |
sahilmgandhi | 14:9e7bb03ddccb | 197 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
sahilmgandhi | 14:9e7bb03ddccb | 198 | count0 = encoder0.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 199 | count1 = encoder1.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 200 | |
sahilmgandhi | 14:9e7bb03ddccb | 201 | error0 = count0 - desiredCount0; |
sahilmgandhi | 14:9e7bb03ddccb | 202 | error1 = count1 - desiredCount1; |
sahilmgandhi | 14:9e7bb03ddccb | 203 | |
sahilmgandhi | 14:9e7bb03ddccb | 204 | right_motor.move(speed0); |
sahilmgandhi | 14:9e7bb03ddccb | 205 | left_motor.move(speed1); |
sahilmgandhi | 14:9e7bb03ddccb | 206 | counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 207 | } else { |
sahilmgandhi | 14:9e7bb03ddccb | 208 | counter++; |
sahilmgandhi | 14:9e7bb03ddccb | 209 | right_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 210 | left_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 211 | } |
sahilmgandhi | 14:9e7bb03ddccb | 212 | |
sahilmgandhi | 14:9e7bb03ddccb | 213 | if (counter > 60) { |
sahilmgandhi | 14:9e7bb03ddccb | 214 | break; |
sahilmgandhi | 14:9e7bb03ddccb | 215 | } |
sahilmgandhi | 14:9e7bb03ddccb | 216 | } |
sahilmgandhi | 14:9e7bb03ddccb | 217 | right_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 218 | left_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 219 | } |
sahilmgandhi | 14:9e7bb03ddccb | 220 | |
sahilmgandhi | 17:f713758f6238 | 221 | void moveForwardCellEncoder(double cellNum){ |
sahilmgandhi | 17:f713758f6238 | 222 | int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum; |
sahilmgandhi | 17:f713758f6238 | 223 | int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum; |
sahilmgandhi | 14:9e7bb03ddccb | 224 | |
sahilmgandhi | 16:d9252437bd92 | 225 | left_motor.forward(0.12); |
sahilmgandhi | 16:d9252437bd92 | 226 | right_motor.forward(0.10); |
sahilmgandhi | 16:d9252437bd92 | 227 | wait_ms(2); |
sahilmgandhi | 16:d9252437bd92 | 228 | while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ |
sahilmgandhi | 17:f713758f6238 | 229 | receiverTwoReading = IRP_2.getSamples(20); |
sahilmgandhi | 17:f713758f6238 | 230 | receiverThreeReading = IRP_3.getSamples(20); |
sahilmgandhi | 17:f713758f6238 | 231 | if (receiverTwoReading <= IRP_2.sensorAvg/2.5) |
sahilmgandhi | 17:f713758f6238 | 232 | turnFlag |= LEFT_FLAG; |
sahilmgandhi | 17:f713758f6238 | 233 | else if (receiverThreeReading <= IRP_3.sensorAvg/2.5) |
sahilmgandhi | 17:f713758f6238 | 234 | turnFlag |= RIGHT_FLAG; |
sahilmgandhi | 16:d9252437bd92 | 235 | // serial.printf("Encoder0 count is: %d, Encoder1 count is: %d, desiredEncoder0 = %d, desiredEncoder1 = %d\n", encoder0.getPulses(), encoder1.getPulses(), desiredCount0, desiredCount1); |
sahilmgandhi | 16:d9252437bd92 | 236 | pidOnEncoders(); |
sahilmgandhi | 16:d9252437bd92 | 237 | } |
sahilmgandhi | 14:9e7bb03ddccb | 238 | |
sahilmgandhi | 16:d9252437bd92 | 239 | left_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 240 | right_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 241 | } |
sahilmgandhi | 14:9e7bb03ddccb | 242 | |
sahilmgandhi | 17:f713758f6238 | 243 | void handleTurns(){ |
sahilmgandhi | 17:f713758f6238 | 244 | if (turnFlag == 0x1){ |
sahilmgandhi | 17:f713758f6238 | 245 | moveForwardCellEncoder(0.5); |
sahilmgandhi | 17:f713758f6238 | 246 | turnLeft(); |
sahilmgandhi | 17:f713758f6238 | 247 | } |
sahilmgandhi | 17:f713758f6238 | 248 | else if (turnFlag == 0x2){ |
sahilmgandhi | 17:f713758f6238 | 249 | moveForwardCellEncoder(0.5); |
sahilmgandhi | 17:f713758f6238 | 250 | turnRight(); |
sahilmgandhi | 17:f713758f6238 | 251 | } |
sahilmgandhi | 17:f713758f6238 | 252 | else if (turnFlag == 0x3){ |
sahilmgandhi | 17:f713758f6238 | 253 | moveForwardCellEncoder(0.5); |
sahilmgandhi | 17:f713758f6238 | 254 | turnLeft(); |
sahilmgandhi | 17:f713758f6238 | 255 | } |
sahilmgandhi | 17:f713758f6238 | 256 | } |
sahilmgandhi | 17:f713758f6238 | 257 | |
sahilmgandhi | 14:9e7bb03ddccb | 258 | void moveForwardUntilWallIr() |
sahilmgandhi | 14:9e7bb03ddccb | 259 | { |
kyleliangus | 9:1d8e4da058cd | 260 | double currentError = 0; |
kyleliangus | 9:1d8e4da058cd | 261 | double previousError = 0; |
kyleliangus | 9:1d8e4da058cd | 262 | double derivError = 0; |
kyleliangus | 9:1d8e4da058cd | 263 | double sumError = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 264 | |
vanshg | 11:8fc2b703086b | 265 | double HIGH_PWM_VOLTAGE = 0.1; |
sahilmgandhi | 14:9e7bb03ddccb | 266 | |
vanshg | 11:8fc2b703086b | 267 | double rightSpeed = 0.1; |
vanshg | 11:8fc2b703086b | 268 | double leftSpeed = 0.1; |
sahilmgandhi | 14:9e7bb03ddccb | 269 | |
kyleliangus | 9:1d8e4da058cd | 270 | float ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 271 | float ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
sahilmgandhi | 14:9e7bb03ddccb | 272 | |
vanshg | 11:8fc2b703086b | 273 | int count = encoder0.getPulses(); |
vanshg | 11:8fc2b703086b | 274 | while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { |
vanshg | 11:8fc2b703086b | 275 | int pulseCount = (encoder0.getPulses()-count) % 5600; |
vanshg | 11:8fc2b703086b | 276 | if (pulseCount > 5400 && pulseCount < 5800) { |
vanshg | 11:8fc2b703086b | 277 | blueLed.write(0); |
vanshg | 11:8fc2b703086b | 278 | } else { |
vanshg | 11:8fc2b703086b | 279 | blueLed.write(1); |
vanshg | 11:8fc2b703086b | 280 | } |
sahilmgandhi | 14:9e7bb03ddccb | 281 | |
kyleliangus | 9:1d8e4da058cd | 282 | currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; |
kyleliangus | 9:1d8e4da058cd | 283 | derivError = currentError - previousError; |
sahilmgandhi | 14:9e7bb03ddccb | 284 | double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; |
kyleliangus | 9:1d8e4da058cd | 285 | if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down |
sahilmgandhi | 14:9e7bb03ddccb | 286 | rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 287 | leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
sahilmgandhi | 14:9e7bb03ddccb | 288 | } else { // r is faster than L. speed up l, slow down r |
sahilmgandhi | 14:9e7bb03ddccb | 289 | rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
sahilmgandhi | 14:9e7bb03ddccb | 290 | leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 291 | } |
sahilmgandhi | 14:9e7bb03ddccb | 292 | |
kyleliangus | 9:1d8e4da058cd | 293 | if (leftSpeed > 0.30) leftSpeed = 0.30; |
kyleliangus | 9:1d8e4da058cd | 294 | if (leftSpeed < 0) leftSpeed = 0; |
kyleliangus | 9:1d8e4da058cd | 295 | if (rightSpeed > 0.30) rightSpeed = 0.30; |
kyleliangus | 9:1d8e4da058cd | 296 | if (rightSpeed < 0) rightSpeed = 0; |
kyleliangus | 9:1d8e4da058cd | 297 | |
sahilmgandhi | 14:9e7bb03ddccb | 298 | right_motor.forward(rightSpeed); |
kyleliangus | 9:1d8e4da058cd | 299 | left_motor.forward(leftSpeed); |
sahilmgandhi | 14:9e7bb03ddccb | 300 | |
kyleliangus | 9:1d8e4da058cd | 301 | previousError = currentError; |
sahilmgandhi | 14:9e7bb03ddccb | 302 | |
kyleliangus | 9:1d8e4da058cd | 303 | ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
sahilmgandhi | 14:9e7bb03ddccb | 304 | ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
sahilmgandhi | 14:9e7bb03ddccb | 305 | |
kyleliangus | 9:1d8e4da058cd | 306 | } |
kyleliangus | 9:1d8e4da058cd | 307 | //sensor1Turn = IR_Sensor1.read(); |
kyleliangus | 9:1d8e4da058cd | 308 | //sensor4Turn = IR_Sensor4.read(); |
sahilmgandhi | 14:9e7bb03ddccb | 309 | |
kyleliangus | 9:1d8e4da058cd | 310 | //backward(); |
kyleliangus | 9:1d8e4da058cd | 311 | wait_ms(40); |
kyleliangus | 9:1d8e4da058cd | 312 | //brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 313 | |
kyleliangus | 9:1d8e4da058cd | 314 | left_motor.brake(); |
kyleliangus | 9:1d8e4da058cd | 315 | right_motor.brake(); |
vanshg | 10:810d1849da9d | 316 | } |
vanshg | 10:810d1849da9d | 317 | |
sahilmgandhi | 14:9e7bb03ddccb | 318 | void printDipFlag() |
sahilmgandhi | 14:9e7bb03ddccb | 319 | { |
vanshg | 11:8fc2b703086b | 320 | if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); |
vanshg | 11:8fc2b703086b | 321 | } |
vanshg | 11:8fc2b703086b | 322 | |
sahilmgandhi | 14:9e7bb03ddccb | 323 | void enableButton1() |
sahilmgandhi | 14:9e7bb03ddccb | 324 | { |
vanshg | 10:810d1849da9d | 325 | dipFlags |= BUTTON1_FLAG; |
vanshg | 11:8fc2b703086b | 326 | printDipFlag(); |
vanshg | 10:810d1849da9d | 327 | } |
sahilmgandhi | 14:9e7bb03ddccb | 328 | void enableButton2() |
sahilmgandhi | 14:9e7bb03ddccb | 329 | { |
vanshg | 10:810d1849da9d | 330 | dipFlags |= BUTTON2_FLAG; |
vanshg | 11:8fc2b703086b | 331 | printDipFlag(); |
vanshg | 10:810d1849da9d | 332 | } |
sahilmgandhi | 14:9e7bb03ddccb | 333 | void enableButton3() |
sahilmgandhi | 14:9e7bb03ddccb | 334 | { |
vanshg | 10:810d1849da9d | 335 | dipFlags |= BUTTON3_FLAG; |
vanshg | 11:8fc2b703086b | 336 | printDipFlag(); |
vanshg | 10:810d1849da9d | 337 | } |
sahilmgandhi | 14:9e7bb03ddccb | 338 | void enableButton4() |
sahilmgandhi | 14:9e7bb03ddccb | 339 | { |
vanshg | 10:810d1849da9d | 340 | dipFlags |= BUTTON4_FLAG; |
vanshg | 11:8fc2b703086b | 341 | printDipFlag(); |
vanshg | 10:810d1849da9d | 342 | } |
sahilmgandhi | 14:9e7bb03ddccb | 343 | void disableButton1() |
sahilmgandhi | 14:9e7bb03ddccb | 344 | { |
vanshg | 10:810d1849da9d | 345 | dipFlags &= ~BUTTON1_FLAG; |
vanshg | 11:8fc2b703086b | 346 | printDipFlag(); |
vanshg | 10:810d1849da9d | 347 | } |
sahilmgandhi | 14:9e7bb03ddccb | 348 | void disableButton2() |
sahilmgandhi | 14:9e7bb03ddccb | 349 | { |
vanshg | 10:810d1849da9d | 350 | dipFlags &= ~BUTTON2_FLAG; |
vanshg | 11:8fc2b703086b | 351 | printDipFlag(); |
vanshg | 10:810d1849da9d | 352 | } |
sahilmgandhi | 14:9e7bb03ddccb | 353 | void disableButton3() |
sahilmgandhi | 14:9e7bb03ddccb | 354 | { |
vanshg | 10:810d1849da9d | 355 | dipFlags &= ~BUTTON3_FLAG; |
vanshg | 11:8fc2b703086b | 356 | printDipFlag(); |
vanshg | 10:810d1849da9d | 357 | } |
sahilmgandhi | 14:9e7bb03ddccb | 358 | void disableButton4() |
sahilmgandhi | 14:9e7bb03ddccb | 359 | { |
vanshg | 10:810d1849da9d | 360 | dipFlags &= ~BUTTON4_FLAG; |
vanshg | 11:8fc2b703086b | 361 | printDipFlag(); |
kyleliangus | 9:1d8e4da058cd | 362 | } |
christine222 | 3:880f15be8c72 | 363 | |
kyleliangus | 15:b80555a4a8b9 | 364 | void pidOnEncoders() |
kyleliangus | 15:b80555a4a8b9 | 365 | { |
kyleliangus | 15:b80555a4a8b9 | 366 | int count0; |
kyleliangus | 15:b80555a4a8b9 | 367 | int count1; |
kyleliangus | 15:b80555a4a8b9 | 368 | count0 = encoder0.getPulses(); |
kyleliangus | 15:b80555a4a8b9 | 369 | count1 = encoder1.getPulses(); |
kyleliangus | 15:b80555a4a8b9 | 370 | int diff = count0 - count1; |
kyleliangus | 15:b80555a4a8b9 | 371 | double kp = 0.000075; |
sahilmgandhi | 16:d9252437bd92 | 372 | double kd = 0.000135; |
kyleliangus | 15:b80555a4a8b9 | 373 | int prev = 0; |
sahilmgandhi | 16:d9252437bd92 | 374 | |
sahilmgandhi | 16:d9252437bd92 | 375 | int counter = 0; |
kyleliangus | 15:b80555a4a8b9 | 376 | while(1) |
kyleliangus | 15:b80555a4a8b9 | 377 | { |
kyleliangus | 15:b80555a4a8b9 | 378 | count0 = encoder0.getPulses(); |
kyleliangus | 15:b80555a4a8b9 | 379 | count1 = encoder1.getPulses(); |
kyleliangus | 15:b80555a4a8b9 | 380 | int x = count0 - count1; |
kyleliangus | 15:b80555a4a8b9 | 381 | //double d = kp * x + kd * ( x - prev ); |
kyleliangus | 15:b80555a4a8b9 | 382 | double kppart = kp * x; |
kyleliangus | 15:b80555a4a8b9 | 383 | double kdpart = kd * (x-prev); |
kyleliangus | 15:b80555a4a8b9 | 384 | double d = kppart + kdpart; |
kyleliangus | 15:b80555a4a8b9 | 385 | |
kyleliangus | 15:b80555a4a8b9 | 386 | //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); |
kyleliangus | 15:b80555a4a8b9 | 387 | if( x < diff - 50 ) // count1 is bigger, right wheel pushed forward |
kyleliangus | 15:b80555a4a8b9 | 388 | { |
kyleliangus | 15:b80555a4a8b9 | 389 | left_motor.move( -d ); |
kyleliangus | 15:b80555a4a8b9 | 390 | right_motor.move( d ); |
kyleliangus | 15:b80555a4a8b9 | 391 | } |
kyleliangus | 15:b80555a4a8b9 | 392 | else if( x > diff + 50 ) |
kyleliangus | 15:b80555a4a8b9 | 393 | { |
kyleliangus | 15:b80555a4a8b9 | 394 | left_motor.move( -d ); |
kyleliangus | 15:b80555a4a8b9 | 395 | right_motor.move( d ); |
kyleliangus | 15:b80555a4a8b9 | 396 | } |
sahilmgandhi | 16:d9252437bd92 | 397 | // else |
sahilmgandhi | 16:d9252437bd92 | 398 | // { |
sahilmgandhi | 16:d9252437bd92 | 399 | // left_motor.brake(); |
sahilmgandhi | 16:d9252437bd92 | 400 | // right_motor.brake(); |
sahilmgandhi | 16:d9252437bd92 | 401 | // } |
kyleliangus | 15:b80555a4a8b9 | 402 | prev = x; |
sahilmgandhi | 16:d9252437bd92 | 403 | counter++; |
sahilmgandhi | 16:d9252437bd92 | 404 | if (counter == 4) |
sahilmgandhi | 16:d9252437bd92 | 405 | break; |
kyleliangus | 15:b80555a4a8b9 | 406 | } |
kyleliangus | 15:b80555a4a8b9 | 407 | } |
kyleliangus | 15:b80555a4a8b9 | 408 | |
sahilmgandhi | 17:f713758f6238 | 409 | void oneCellEncoderAndIR(){ |
sahilmgandhi | 17:f713758f6238 | 410 | |
sahilmgandhi | 17:f713758f6238 | 411 | } |
sahilmgandhi | 17:f713758f6238 | 412 | |
sahilmgandhi | 0:a03c771ab78e | 413 | int main() |
sahilmgandhi | 0:a03c771ab78e | 414 | { |
christine222 | 3:880f15be8c72 | 415 | //Set highest bandwidth. |
sahilmgandhi | 1:8a4b2f573923 | 416 | gyro.setLpBandwidth(LPFBW_42HZ); |
christine222 | 3:880f15be8c72 | 417 | serial.baud(9600); |
sahilmgandhi | 7:6f5cb6377bd4 | 418 | serial.printf("The gyro's address is %s", gyro.getWhoAmI()); |
sahilmgandhi | 7:6f5cb6377bd4 | 419 | |
sahilmgandhi | 1:8a4b2f573923 | 420 | wait (0.1); |
sahilmgandhi | 7:6f5cb6377bd4 | 421 | |
christine222 | 3:880f15be8c72 | 422 | |
sahilmgandhi | 2:771db996cee0 | 423 | redLed.write(1); |
sahilmgandhi | 14:9e7bb03ddccb | 424 | greenLed.write(0); |
sahilmgandhi | 2:771db996cee0 | 425 | blueLed.write(1); |
sahilmgandhi | 14:9e7bb03ddccb | 426 | |
kyleliangus | 9:1d8e4da058cd | 427 | //left_motor.forward(0.1); |
kyleliangus | 9:1d8e4da058cd | 428 | //right_motor.forward(0.1); |
kyleliangus | 8:a0760acdc59e | 429 | |
kyleliangus | 8:a0760acdc59e | 430 | // PA_1 is A of right |
kyleliangus | 8:a0760acdc59e | 431 | // PA_0 is B of right |
kyleliangus | 8:a0760acdc59e | 432 | // PA_5 is A of left |
kyleliangus | 8:a0760acdc59e | 433 | // PB_3 is B of left |
vanshg | 11:8fc2b703086b | 434 | //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING ); |
vanshg | 11:8fc2b703086b | 435 | // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); |
sahilmgandhi | 14:9e7bb03ddccb | 436 | |
vanshg | 10:810d1849da9d | 437 | // TODO: Setting all the registers and what not for Quadrature Encoders |
sahilmgandhi | 14:9e7bb03ddccb | 438 | /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3) |
sahilmgandhi | 14:9e7bb03ddccb | 439 | RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B) |
sahilmgandhi | 14:9e7bb03ddccb | 440 | GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2 |
sahilmgandhi | 14:9e7bb03ddccb | 441 | GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5 |
sahilmgandhi | 14:9e7bb03ddccb | 442 | */ |
sahilmgandhi | 14:9e7bb03ddccb | 443 | |
kyleliangus | 12:5790e56a056f | 444 | // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type |
kyleliangus | 12:5790e56a056f | 445 | // of alternate function specified |
kyleliangus | 12:5790e56a056f | 446 | // 4 modes sets AHB1ENR |
kyleliangus | 12:5790e56a056f | 447 | // Now TMR: enable clock with timer, APB1ENR |
kyleliangus | 12:5790e56a056f | 448 | // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN |
kyleliangus | 12:5790e56a056f | 449 | // |
kyleliangus | 12:5790e56a056f | 450 | // Encoder mode: disable timer before changing timer to encoder |
kyleliangus | 12:5790e56a056f | 451 | // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use |
kyleliangus | 12:5790e56a056f | 452 | // CCMR sets sample rate and set the channel to input |
kyleliangus | 12:5790e56a056f | 453 | // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels |
kyleliangus | 12:5790e56a056f | 454 | // SMCR - encoder mode |
kyleliangus | 12:5790e56a056f | 455 | // CR1 reenabales |
kyleliangus | 12:5790e56a056f | 456 | // then read CNT reg of timer |
sahilmgandhi | 14:9e7bb03ddccb | 457 | |
sahilmgandhi | 14:9e7bb03ddccb | 458 | |
vanshg | 10:810d1849da9d | 459 | dipButton1.rise(&enableButton1); |
vanshg | 10:810d1849da9d | 460 | dipButton2.rise(&enableButton2); |
vanshg | 10:810d1849da9d | 461 | dipButton3.rise(&enableButton3); |
vanshg | 10:810d1849da9d | 462 | dipButton4.rise(&enableButton4); |
sahilmgandhi | 14:9e7bb03ddccb | 463 | |
vanshg | 10:810d1849da9d | 464 | dipButton1.fall(&disableButton1); |
vanshg | 10:810d1849da9d | 465 | dipButton2.fall(&disableButton2); |
vanshg | 10:810d1849da9d | 466 | dipButton3.fall(&disableButton3); |
vanshg | 10:810d1849da9d | 467 | dipButton4.fall(&disableButton4); |
sahilmgandhi | 7:6f5cb6377bd4 | 468 | |
kyleliangus | 15:b80555a4a8b9 | 469 | //right_motor.forward( 0.2 ); |
kyleliangus | 15:b80555a4a8b9 | 470 | //left_motor.forward( 0.2 ); |
kyleliangus | 15:b80555a4a8b9 | 471 | |
christine222 | 3:880f15be8c72 | 472 | while (1) { |
sahilmgandhi | 17:f713758f6238 | 473 | moveForwardCellEncoder(1); |
sahilmgandhi | 17:f713758f6238 | 474 | wait(0.3); |
sahilmgandhi | 17:f713758f6238 | 475 | handleTurns(); |
sahilmgandhi | 17:f713758f6238 | 476 | wait_ms(5); |
sahilmgandhi | 17:f713758f6238 | 477 | moveForwardCellEncoder(1); |
sahilmgandhi | 17:f713758f6238 | 478 | wait(0.3); |
sahilmgandhi | 17:f713758f6238 | 479 | handleTurns(); |
sahilmgandhi | 17:f713758f6238 | 480 | break; |
sahilmgandhi | 17:f713758f6238 | 481 | // moveForwardOneCellEncoder(); |
sahilmgandhi | 16:d9252437bd92 | 482 | //pidOnEncoders(); |
kyleliangus | 15:b80555a4a8b9 | 483 | //moveForwardUntilWallIr(); |
sahilmgandhi | 14:9e7bb03ddccb | 484 | // wait(2); |
sahilmgandhi | 14:9e7bb03ddccb | 485 | // turnRight(); |
sahilmgandhi | 14:9e7bb03ddccb | 486 | // wait(1); |
sahilmgandhi | 14:9e7bb03ddccb | 487 | // moveForwardUntilWallIr(); |
kyleliangus | 8:a0760acdc59e | 488 | //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ()); |
kyleliangus | 15:b80555a4a8b9 | 489 | //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); |
sahilmgandhi | 17:f713758f6238 | 490 | // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; |
sahilmgandhi | 17:f713758f6238 | 491 | // serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", |
sahilmgandhi | 17:f713758f6238 | 492 | // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); |
sahilmgandhi | 14:9e7bb03ddccb | 493 | |
christine222 | 3:880f15be8c72 | 494 | //reading = Rec_4.read(); |
christine222 | 3:880f15be8c72 | 495 | // serial.printf("reading: %f\n", reading); |
sahilmgandhi | 1:8a4b2f573923 | 496 | // redLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 497 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 498 | // redLed.write(1); |
sahilmgandhi | 1:8a4b2f573923 | 499 | // greenLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 500 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 501 | // greenLed.write(1); |
sahilmgandhi | 1:8a4b2f573923 | 502 | // blueLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 503 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 504 | // blueLed.write(1); |
christine222 | 3:880f15be8c72 | 505 | } |
sahilmgandhi | 0:a03c771ab78e | 506 | } |