Mouse code for the MacroRat
main.cpp@13:2032db00f168, 2017-05-13 (annotated)
- Committer:
- christine222
- Date:
- Sat May 13 02:40:49 2017 +0000
- Revision:
- 13:2032db00f168
- Parent:
- 12:5790e56a056f
- Child:
- 14:9e7bb03ddccb
turning sort of works
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" |
vanshg | 10:810d1849da9d | 9 | #include "stm32f4xx.h" |
sahilmgandhi | 7:6f5cb6377bd4 | 10 | #include "QEI.h" |
sahilmgandhi | 0:a03c771ab78e | 11 | |
kyleliangus | 9:1d8e4da058cd | 12 | |
vanshg | 11:8fc2b703086b | 13 | /* Constants for when HIGH_PWM_VOLTAGE = 0.2 |
kyleliangus | 9:1d8e4da058cd | 14 | #define IP_CONSTANT 6 |
kyleliangus | 9:1d8e4da058cd | 15 | #define II_CONSTANT 0 |
kyleliangus | 9:1d8e4da058cd | 16 | #define ID_CONSTANT 1 |
vanshg | 11:8fc2b703086b | 17 | */ |
sahilmgandhi | 0:a03c771ab78e | 18 | |
vanshg | 11:8fc2b703086b | 19 | // Constants for when HIGH_PWM_VOLTAGE = 0.1 |
christine222 | 13:2032db00f168 | 20 | #define IP_CONSTANT 13 |
vanshg | 11:8fc2b703086b | 21 | #define II_CONSTANT 0 |
christine222 | 13:2032db00f168 | 22 | #define ID_CONSTANT 1. |
christine222 | 13:2032db00f168 | 23 | |
christine222 | 13:2032db00f168 | 24 | const int desiredCountR = 1300; |
christine222 | 13:2032db00f168 | 25 | const int desiredCountL = 1400; |
christine222 | 13:2032db00f168 | 26 | |
christine222 | 13:2032db00f168 | 27 | void turnLeft(){ |
christine222 | 13:2032db00f168 | 28 | double speed0 = 0.15; |
christine222 | 13:2032db00f168 | 29 | double speed1 = 0.15; |
christine222 | 13:2032db00f168 | 30 | double kp = 0.01; |
christine222 | 13:2032db00f168 | 31 | |
christine222 | 13:2032db00f168 | 32 | int counter = 0; |
christine222 | 13:2032db00f168 | 33 | |
christine222 | 13:2032db00f168 | 34 | int initialCount0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 35 | int initialCount1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 36 | |
christine222 | 13:2032db00f168 | 37 | double desiredCount0 = initialCount0 - desiredCountL; |
christine222 | 13:2032db00f168 | 38 | double desiredCount1 = initialCount1 + desiredCountL; |
christine222 | 13:2032db00f168 | 39 | |
christine222 | 13:2032db00f168 | 40 | int count0 = initialCount0; |
christine222 | 13:2032db00f168 | 41 | int count1 = initialCount1; |
christine222 | 13:2032db00f168 | 42 | |
christine222 | 13:2032db00f168 | 43 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 44 | double error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 45 | |
christine222 | 13:2032db00f168 | 46 | while(1){ |
christine222 | 13:2032db00f168 | 47 | |
christine222 | 13:2032db00f168 | 48 | if(!(abs(error0) < 1) && !(abs(error1) < 1)){ |
christine222 | 13:2032db00f168 | 49 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 50 | count1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 51 | |
christine222 | 13:2032db00f168 | 52 | error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 53 | error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 54 | |
christine222 | 13:2032db00f168 | 55 | speed0 = error0 * kp + speed0; |
christine222 | 13:2032db00f168 | 56 | speed1 = error1 * kp + speed1; |
christine222 | 13:2032db00f168 | 57 | |
christine222 | 13:2032db00f168 | 58 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 59 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 60 | counter = 0; |
christine222 | 13:2032db00f168 | 61 | }else{ |
christine222 | 13:2032db00f168 | 62 | counter++; |
christine222 | 13:2032db00f168 | 63 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 64 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 65 | } |
christine222 | 13:2032db00f168 | 66 | |
christine222 | 13:2032db00f168 | 67 | if (counter > 60){ |
christine222 | 13:2032db00f168 | 68 | break; |
christine222 | 13:2032db00f168 | 69 | } |
christine222 | 13:2032db00f168 | 70 | } |
christine222 | 13:2032db00f168 | 71 | |
christine222 | 13:2032db00f168 | 72 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 73 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 74 | } |
kyleliangus | 9:1d8e4da058cd | 75 | |
kyleliangus | 12:5790e56a056f | 76 | void turnRight() { //e1 should be negative encoder0 is left, encoder1 is right |
kyleliangus | 12:5790e56a056f | 77 | double speed0 = 0.15; |
kyleliangus | 12:5790e56a056f | 78 | double speed1 = 0.15; |
christine222 | 13:2032db00f168 | 79 | double kp = 0.01; |
christine222 | 13:2032db00f168 | 80 | |
christine222 | 13:2032db00f168 | 81 | int counter = 0; |
christine222 | 13:2032db00f168 | 82 | |
christine222 | 13:2032db00f168 | 83 | int initialCount0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 84 | int initialCount1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 85 | |
christine222 | 13:2032db00f168 | 86 | double desiredCount0 = initialCount0 + desiredCountR; |
christine222 | 13:2032db00f168 | 87 | double desiredCount1 = initialCount1 - desiredCountR; |
christine222 | 13:2032db00f168 | 88 | |
christine222 | 13:2032db00f168 | 89 | int count0 = initialCount0; |
christine222 | 13:2032db00f168 | 90 | int count1 = initialCount1; |
kyleliangus | 12:5790e56a056f | 91 | |
christine222 | 13:2032db00f168 | 92 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 93 | double error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 94 | |
christine222 | 13:2032db00f168 | 95 | while(1){ |
christine222 | 13:2032db00f168 | 96 | |
christine222 | 13:2032db00f168 | 97 | if(!(abs(error0) < 1) && !(abs(error1) < 1)){ |
christine222 | 13:2032db00f168 | 98 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 99 | count1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 100 | |
christine222 | 13:2032db00f168 | 101 | error0 = count0 - desiredCount0; // moves forward |
christine222 | 13:2032db00f168 | 102 | error1 = count1 - desiredCount1; // moves backwards |
christine222 | 13:2032db00f168 | 103 | |
christine222 | 13:2032db00f168 | 104 | speed0 = error0 * kp + speed0; |
christine222 | 13:2032db00f168 | 105 | speed1 = error1 * kp + speed1; |
kyleliangus | 12:5790e56a056f | 106 | |
christine222 | 13:2032db00f168 | 107 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 108 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 109 | counter = 0; |
christine222 | 13:2032db00f168 | 110 | }else{ |
christine222 | 13:2032db00f168 | 111 | counter++; |
christine222 | 13:2032db00f168 | 112 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 113 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 114 | } |
kyleliangus | 12:5790e56a056f | 115 | |
christine222 | 13:2032db00f168 | 116 | if (counter > 60){ |
christine222 | 13:2032db00f168 | 117 | break; |
christine222 | 13:2032db00f168 | 118 | } |
christine222 | 13:2032db00f168 | 119 | |
christine222 | 13:2032db00f168 | 120 | // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1); |
christine222 | 13:2032db00f168 | 121 | // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(), encoder1.getPulses()); |
christine222 | 13:2032db00f168 | 122 | } |
christine222 | 13:2032db00f168 | 123 | |
christine222 | 13:2032db00f168 | 124 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 125 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 126 | } |
christine222 | 13:2032db00f168 | 127 | |
christine222 | 13:2032db00f168 | 128 | void turnLeft180(){ |
christine222 | 13:2032db00f168 | 129 | double speed0 = 0.15; |
christine222 | 13:2032db00f168 | 130 | double speed1 = 0.15; |
christine222 | 13:2032db00f168 | 131 | double kp = 0.01; |
christine222 | 13:2032db00f168 | 132 | |
christine222 | 13:2032db00f168 | 133 | int counter = 0; |
kyleliangus | 12:5790e56a056f | 134 | |
kyleliangus | 12:5790e56a056f | 135 | int initialCount0 = encoder0.getPulses(); |
kyleliangus | 12:5790e56a056f | 136 | int initialCount1 = encoder1.getPulses(); |
kyleliangus | 12:5790e56a056f | 137 | |
christine222 | 13:2032db00f168 | 138 | double desiredCount0 = initialCount0 - 3000; |
christine222 | 13:2032db00f168 | 139 | double desiredCount1 = initialCount1 + 2700; |
christine222 | 13:2032db00f168 | 140 | |
christine222 | 13:2032db00f168 | 141 | int count0 = initialCount0; |
christine222 | 13:2032db00f168 | 142 | int count1 = initialCount1; |
christine222 | 13:2032db00f168 | 143 | |
christine222 | 13:2032db00f168 | 144 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 145 | double error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 146 | |
christine222 | 13:2032db00f168 | 147 | while(1){ |
christine222 | 13:2032db00f168 | 148 | |
christine222 | 13:2032db00f168 | 149 | if(!(abs(error0) < 1) && !(abs(error1) < 1)){ |
christine222 | 13:2032db00f168 | 150 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 151 | count1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 152 | |
christine222 | 13:2032db00f168 | 153 | error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 154 | error1 = count1 - desiredCount1; |
kyleliangus | 12:5790e56a056f | 155 | |
christine222 | 13:2032db00f168 | 156 | speed0 = error0 * kp + speed0; |
christine222 | 13:2032db00f168 | 157 | speed1 = error1 * kp + speed1; |
kyleliangus | 12:5790e56a056f | 158 | |
christine222 | 13:2032db00f168 | 159 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 160 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 161 | counter = 0; |
christine222 | 13:2032db00f168 | 162 | }else{ |
christine222 | 13:2032db00f168 | 163 | counter++; |
christine222 | 13:2032db00f168 | 164 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 165 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 166 | } |
christine222 | 13:2032db00f168 | 167 | |
christine222 | 13:2032db00f168 | 168 | if (counter > 60){ |
christine222 | 13:2032db00f168 | 169 | break; |
christine222 | 13:2032db00f168 | 170 | } |
christine222 | 13:2032db00f168 | 171 | } |
christine222 | 13:2032db00f168 | 172 | |
christine222 | 13:2032db00f168 | 173 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 174 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 175 | } |
kyleliangus | 12:5790e56a056f | 176 | |
christine222 | 13:2032db00f168 | 177 | void turnRight180(){ |
christine222 | 13:2032db00f168 | 178 | double speed0 = 0.15; |
christine222 | 13:2032db00f168 | 179 | double speed1 = 0.15; |
christine222 | 13:2032db00f168 | 180 | double kp = 0.01; |
christine222 | 13:2032db00f168 | 181 | |
christine222 | 13:2032db00f168 | 182 | int counter = 0; |
christine222 | 13:2032db00f168 | 183 | |
christine222 | 13:2032db00f168 | 184 | int initialCount0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 185 | int initialCount1 = encoder1.getPulses(); |
kyleliangus | 12:5790e56a056f | 186 | |
christine222 | 13:2032db00f168 | 187 | double desiredCount0 = initialCount0 + desiredCountR*2; |
christine222 | 13:2032db00f168 | 188 | double desiredCount1 = initialCount1 - desiredCountR*2; |
christine222 | 13:2032db00f168 | 189 | |
christine222 | 13:2032db00f168 | 190 | int count0 = initialCount0; |
christine222 | 13:2032db00f168 | 191 | int count1 = initialCount1; |
christine222 | 13:2032db00f168 | 192 | |
christine222 | 13:2032db00f168 | 193 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 194 | double error1 = count1 - desiredCount1; |
christine222 | 13:2032db00f168 | 195 | |
christine222 | 13:2032db00f168 | 196 | while(1){ |
kyleliangus | 12:5790e56a056f | 197 | |
christine222 | 13:2032db00f168 | 198 | if(!(abs(error0) < 1) && !(abs(error1) < 1)){ |
christine222 | 13:2032db00f168 | 199 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 200 | count1 = encoder1.getPulses(); |
christine222 | 13:2032db00f168 | 201 | |
christine222 | 13:2032db00f168 | 202 | error0 = count0 - desiredCount0; // moves forward |
christine222 | 13:2032db00f168 | 203 | error1 = count1 - desiredCount1; // moves backwards |
christine222 | 13:2032db00f168 | 204 | |
christine222 | 13:2032db00f168 | 205 | speed0 = error0 * kp + speed0; |
christine222 | 13:2032db00f168 | 206 | speed1 = error1 * kp + speed1; |
kyleliangus | 12:5790e56a056f | 207 | |
christine222 | 13:2032db00f168 | 208 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 209 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 210 | counter = 0; |
christine222 | 13:2032db00f168 | 211 | }else{ |
christine222 | 13:2032db00f168 | 212 | counter++; |
christine222 | 13:2032db00f168 | 213 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 214 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 215 | } |
christine222 | 13:2032db00f168 | 216 | |
christine222 | 13:2032db00f168 | 217 | if (counter > 60){ |
christine222 | 13:2032db00f168 | 218 | break; |
christine222 | 13:2032db00f168 | 219 | } |
christine222 | 13:2032db00f168 | 220 | |
christine222 | 13:2032db00f168 | 221 | // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1); |
christine222 | 13:2032db00f168 | 222 | // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(), encoder1.getPulses()); |
kyleliangus | 12:5790e56a056f | 223 | } |
kyleliangus | 12:5790e56a056f | 224 | |
kyleliangus | 12:5790e56a056f | 225 | right_motor.brake(); |
kyleliangus | 12:5790e56a056f | 226 | left_motor.brake(); |
kyleliangus | 12:5790e56a056f | 227 | } |
kyleliangus | 12:5790e56a056f | 228 | |
kyleliangus | 9:1d8e4da058cd | 229 | void moveForwardUntilWallIr() { |
kyleliangus | 9:1d8e4da058cd | 230 | double currentError = 0; |
kyleliangus | 9:1d8e4da058cd | 231 | double previousError = 0; |
kyleliangus | 9:1d8e4da058cd | 232 | double derivError = 0; |
kyleliangus | 9:1d8e4da058cd | 233 | double sumError = 0; |
kyleliangus | 9:1d8e4da058cd | 234 | |
vanshg | 11:8fc2b703086b | 235 | double HIGH_PWM_VOLTAGE = 0.1; |
kyleliangus | 9:1d8e4da058cd | 236 | |
vanshg | 11:8fc2b703086b | 237 | double rightSpeed = 0.1; |
vanshg | 11:8fc2b703086b | 238 | double leftSpeed = 0.1; |
kyleliangus | 9:1d8e4da058cd | 239 | |
kyleliangus | 9:1d8e4da058cd | 240 | float ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 241 | float ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 242 | |
vanshg | 11:8fc2b703086b | 243 | int count = encoder0.getPulses(); |
vanshg | 11:8fc2b703086b | 244 | while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { |
vanshg | 11:8fc2b703086b | 245 | int pulseCount = (encoder0.getPulses()-count) % 5600; |
vanshg | 11:8fc2b703086b | 246 | if (pulseCount > 5400 && pulseCount < 5800) { |
vanshg | 11:8fc2b703086b | 247 | blueLed.write(0); |
vanshg | 11:8fc2b703086b | 248 | } else { |
vanshg | 11:8fc2b703086b | 249 | blueLed.write(1); |
vanshg | 11:8fc2b703086b | 250 | } |
vanshg | 11:8fc2b703086b | 251 | |
vanshg | 11:8fc2b703086b | 252 | |
kyleliangus | 9:1d8e4da058cd | 253 | currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; |
kyleliangus | 9:1d8e4da058cd | 254 | derivError = currentError - previousError; |
kyleliangus | 9:1d8e4da058cd | 255 | double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; |
kyleliangus | 9:1d8e4da058cd | 256 | if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down |
kyleliangus | 9:1d8e4da058cd | 257 | rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 258 | leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 259 | } else { // r is faster than L. speed up l, slow down r |
kyleliangus | 9:1d8e4da058cd | 260 | rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 261 | leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 262 | } |
kyleliangus | 9:1d8e4da058cd | 263 | |
kyleliangus | 9:1d8e4da058cd | 264 | if (leftSpeed > 0.30) leftSpeed = 0.30; |
kyleliangus | 9:1d8e4da058cd | 265 | if (leftSpeed < 0) leftSpeed = 0; |
kyleliangus | 9:1d8e4da058cd | 266 | if (rightSpeed > 0.30) rightSpeed = 0.30; |
kyleliangus | 9:1d8e4da058cd | 267 | if (rightSpeed < 0) rightSpeed = 0; |
kyleliangus | 9:1d8e4da058cd | 268 | |
kyleliangus | 9:1d8e4da058cd | 269 | right_motor.forward(rightSpeed); |
kyleliangus | 9:1d8e4da058cd | 270 | left_motor.forward(leftSpeed); |
kyleliangus | 9:1d8e4da058cd | 271 | |
kyleliangus | 9:1d8e4da058cd | 272 | previousError = currentError; |
kyleliangus | 9:1d8e4da058cd | 273 | |
kyleliangus | 9:1d8e4da058cd | 274 | ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 275 | ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 276 | |
kyleliangus | 9:1d8e4da058cd | 277 | } |
kyleliangus | 9:1d8e4da058cd | 278 | //sensor1Turn = IR_Sensor1.read(); |
kyleliangus | 9:1d8e4da058cd | 279 | //sensor4Turn = IR_Sensor4.read(); |
kyleliangus | 9:1d8e4da058cd | 280 | |
kyleliangus | 9:1d8e4da058cd | 281 | //backward(); |
kyleliangus | 9:1d8e4da058cd | 282 | wait_ms(40); |
kyleliangus | 9:1d8e4da058cd | 283 | //brake(); |
kyleliangus | 9:1d8e4da058cd | 284 | |
kyleliangus | 9:1d8e4da058cd | 285 | left_motor.brake(); |
kyleliangus | 9:1d8e4da058cd | 286 | right_motor.brake(); |
vanshg | 10:810d1849da9d | 287 | } |
vanshg | 10:810d1849da9d | 288 | |
vanshg | 11:8fc2b703086b | 289 | void printDipFlag() { |
vanshg | 11:8fc2b703086b | 290 | if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); |
vanshg | 11:8fc2b703086b | 291 | } |
vanshg | 11:8fc2b703086b | 292 | |
vanshg | 10:810d1849da9d | 293 | void enableButton1() { |
vanshg | 10:810d1849da9d | 294 | dipFlags |= BUTTON1_FLAG; |
vanshg | 11:8fc2b703086b | 295 | printDipFlag(); |
vanshg | 10:810d1849da9d | 296 | } |
vanshg | 10:810d1849da9d | 297 | void enableButton2() { |
vanshg | 10:810d1849da9d | 298 | dipFlags |= BUTTON2_FLAG; |
vanshg | 11:8fc2b703086b | 299 | printDipFlag(); |
vanshg | 10:810d1849da9d | 300 | } |
vanshg | 10:810d1849da9d | 301 | void enableButton3() { |
vanshg | 10:810d1849da9d | 302 | dipFlags |= BUTTON3_FLAG; |
vanshg | 11:8fc2b703086b | 303 | printDipFlag(); |
vanshg | 10:810d1849da9d | 304 | } |
vanshg | 10:810d1849da9d | 305 | void enableButton4() { |
vanshg | 10:810d1849da9d | 306 | dipFlags |= BUTTON4_FLAG; |
vanshg | 11:8fc2b703086b | 307 | printDipFlag(); |
vanshg | 10:810d1849da9d | 308 | } |
vanshg | 10:810d1849da9d | 309 | void disableButton1() { |
vanshg | 10:810d1849da9d | 310 | dipFlags &= ~BUTTON1_FLAG; |
vanshg | 11:8fc2b703086b | 311 | printDipFlag(); |
vanshg | 10:810d1849da9d | 312 | } |
vanshg | 10:810d1849da9d | 313 | void disableButton2() { |
vanshg | 10:810d1849da9d | 314 | dipFlags &= ~BUTTON2_FLAG; |
vanshg | 11:8fc2b703086b | 315 | printDipFlag(); |
vanshg | 10:810d1849da9d | 316 | } |
vanshg | 10:810d1849da9d | 317 | void disableButton3() { |
vanshg | 10:810d1849da9d | 318 | dipFlags &= ~BUTTON3_FLAG; |
vanshg | 11:8fc2b703086b | 319 | printDipFlag(); |
vanshg | 10:810d1849da9d | 320 | } |
vanshg | 10:810d1849da9d | 321 | void disableButton4() { |
vanshg | 10:810d1849da9d | 322 | dipFlags &= ~BUTTON4_FLAG; |
vanshg | 11:8fc2b703086b | 323 | printDipFlag(); |
kyleliangus | 9:1d8e4da058cd | 324 | } |
christine222 | 3:880f15be8c72 | 325 | |
sahilmgandhi | 0:a03c771ab78e | 326 | int main() |
sahilmgandhi | 0:a03c771ab78e | 327 | { |
kyleliangus | 8:a0760acdc59e | 328 | //enableMotors(); |
christine222 | 3:880f15be8c72 | 329 | //Set highest bandwidth. |
sahilmgandhi | 1:8a4b2f573923 | 330 | gyro.setLpBandwidth(LPFBW_42HZ); |
christine222 | 3:880f15be8c72 | 331 | serial.baud(9600); |
sahilmgandhi | 7:6f5cb6377bd4 | 332 | serial.printf("The gyro's address is %s", gyro.getWhoAmI()); |
sahilmgandhi | 7:6f5cb6377bd4 | 333 | |
sahilmgandhi | 1:8a4b2f573923 | 334 | wait (0.1); |
sahilmgandhi | 7:6f5cb6377bd4 | 335 | |
christine222 | 3:880f15be8c72 | 336 | // IR_1.write(1); |
sahilmgandhi | 2:771db996cee0 | 337 | // IR_2.write(1); |
sahilmgandhi | 2:771db996cee0 | 338 | // IR_3.write(1); |
sahilmgandhi | 2:771db996cee0 | 339 | // IR_4.write(1); |
christine222 | 3:880f15be8c72 | 340 | |
sahilmgandhi | 2:771db996cee0 | 341 | redLed.write(1); |
vanshg | 11:8fc2b703086b | 342 | greenLed.write(1); |
sahilmgandhi | 2:771db996cee0 | 343 | blueLed.write(1); |
sahilmgandhi | 1:8a4b2f573923 | 344 | |
kyleliangus | 9:1d8e4da058cd | 345 | //left_motor.forward(0.1); |
kyleliangus | 9:1d8e4da058cd | 346 | //right_motor.forward(0.1); |
kyleliangus | 8:a0760acdc59e | 347 | |
kyleliangus | 8:a0760acdc59e | 348 | // PA_1 is A of right |
kyleliangus | 8:a0760acdc59e | 349 | // PA_0 is B of right |
kyleliangus | 8:a0760acdc59e | 350 | // PA_5 is A of left |
kyleliangus | 8:a0760acdc59e | 351 | // PB_3 is B of left |
vanshg | 11:8fc2b703086b | 352 | //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING ); |
vanshg | 11:8fc2b703086b | 353 | // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); |
vanshg | 10:810d1849da9d | 354 | |
vanshg | 10:810d1849da9d | 355 | // TODO: Setting all the registers and what not for Quadrature Encoders |
christine222 | 13:2032db00f168 | 356 | /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3) |
vanshg | 10:810d1849da9d | 357 | RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B) |
vanshg | 10:810d1849da9d | 358 | GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2 |
vanshg | 10:810d1849da9d | 359 | GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5 |
christine222 | 13:2032db00f168 | 360 | */ |
vanshg | 10:810d1849da9d | 361 | |
kyleliangus | 12:5790e56a056f | 362 | // 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 | 363 | // of alternate function specified |
kyleliangus | 12:5790e56a056f | 364 | // 4 modes sets AHB1ENR |
kyleliangus | 12:5790e56a056f | 365 | // Now TMR: enable clock with timer, APB1ENR |
kyleliangus | 12:5790e56a056f | 366 | // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN |
kyleliangus | 12:5790e56a056f | 367 | // |
kyleliangus | 12:5790e56a056f | 368 | // Encoder mode: disable timer before changing timer to encoder |
kyleliangus | 12:5790e56a056f | 369 | // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use |
kyleliangus | 12:5790e56a056f | 370 | // CCMR sets sample rate and set the channel to input |
kyleliangus | 12:5790e56a056f | 371 | // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels |
kyleliangus | 12:5790e56a056f | 372 | // SMCR - encoder mode |
kyleliangus | 12:5790e56a056f | 373 | // CR1 reenabales |
kyleliangus | 12:5790e56a056f | 374 | // then read CNT reg of timer |
kyleliangus | 12:5790e56a056f | 375 | |
kyleliangus | 12:5790e56a056f | 376 | |
vanshg | 10:810d1849da9d | 377 | dipButton1.rise(&enableButton1); |
vanshg | 10:810d1849da9d | 378 | dipButton2.rise(&enableButton2); |
vanshg | 10:810d1849da9d | 379 | dipButton3.rise(&enableButton3); |
vanshg | 10:810d1849da9d | 380 | dipButton4.rise(&enableButton4); |
vanshg | 10:810d1849da9d | 381 | |
vanshg | 10:810d1849da9d | 382 | dipButton1.fall(&disableButton1); |
vanshg | 10:810d1849da9d | 383 | dipButton2.fall(&disableButton2); |
vanshg | 10:810d1849da9d | 384 | dipButton3.fall(&disableButton3); |
vanshg | 10:810d1849da9d | 385 | dipButton4.fall(&disableButton4); |
vanshg | 10:810d1849da9d | 386 | |
sahilmgandhi | 7:6f5cb6377bd4 | 387 | |
christine222 | 3:880f15be8c72 | 388 | while (1) { |
vanshg | 11:8fc2b703086b | 389 | |
christine222 | 13:2032db00f168 | 390 | // moveForwardUntilWallIr(); |
christine222 | 13:2032db00f168 | 391 | // wait(2); |
christine222 | 13:2032db00f168 | 392 | //turnRight180(); |
christine222 | 13:2032db00f168 | 393 | wait(0.5); |
christine222 | 13:2032db00f168 | 394 | turnLeft180(); |
christine222 | 13:2032db00f168 | 395 | wait(1); |
christine222 | 13:2032db00f168 | 396 | //turnRight180(); |
kyleliangus | 12:5790e56a056f | 397 | wait(1); |
christine222 | 13:2032db00f168 | 398 | //turnLeft(); |
christine222 | 13:2032db00f168 | 399 | |
christine222 | 13:2032db00f168 | 400 | // wait(1); |
christine222 | 13:2032db00f168 | 401 | // turnLeft(); |
christine222 | 13:2032db00f168 | 402 | // wait(1); |
christine222 | 13:2032db00f168 | 403 | // turnRight(); |
christine222 | 13:2032db00f168 | 404 | // wait(1); |
christine222 | 13:2032db00f168 | 405 | // turnRight(); |
christine222 | 13:2032db00f168 | 406 | // turnRight180(); |
christine222 | 13:2032db00f168 | 407 | // wait(1); |
christine222 | 13:2032db00f168 | 408 | // turnLeft180(); |
christine222 | 13:2032db00f168 | 409 | // wait(1); |
christine222 | 13:2032db00f168 | 410 | break; |
kyleliangus | 8:a0760acdc59e | 411 | //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ()); |
vanshg | 11:8fc2b703086b | 412 | // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); |
kyleliangus | 9:1d8e4da058cd | 413 | //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; |
kyleliangus | 9:1d8e4da058cd | 414 | |
kyleliangus | 9:1d8e4da058cd | 415 | //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", |
kyleliangus | 9:1d8e4da058cd | 416 | // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); |
kyleliangus | 9:1d8e4da058cd | 417 | |
christine222 | 3:880f15be8c72 | 418 | //reading = Rec_4.read(); |
christine222 | 3:880f15be8c72 | 419 | // serial.printf("reading: %f\n", reading); |
sahilmgandhi | 1:8a4b2f573923 | 420 | // redLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 421 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 422 | // redLed.write(1); |
sahilmgandhi | 1:8a4b2f573923 | 423 | // greenLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 424 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 425 | // greenLed.write(1); |
sahilmgandhi | 1:8a4b2f573923 | 426 | // blueLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 427 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 428 | // blueLed.write(1); |
christine222 | 3:880f15be8c72 | 429 | } |
sahilmgandhi | 0:a03c771ab78e | 430 | } |