MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@15:b80555a4a8b9, 2017-05-14 (annotated)

Committer:
kyleliangus
Date:
Sun May 14 04:45:21 2017 +0000
Revision:
15:b80555a4a8b9
Parent:
14:9e7bb03ddccb
Child:
16:d9252437bd92
Created PID for encoder based on difference of speed rotations.

Who changed what in which revision?

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