MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@19:7b66a518b6f8, 2017-05-15 (annotated)

Committer:
sahilmgandhi
Date:
Mon May 15 00:54:41 2017 +0000
Revision:
19:7b66a518b6f8
Parent:
18:6a4db94011d3
Child:
20:82836745332e
Got the PID to work on both encoder and IR at the same time ... BUT i still cannot get it to recognize it to turn the right direction.

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
sahilmgandhi 19:7b66a518b6f8 19 #define IP_CONSTANT 8.9
kyleliangus 15:b80555a4a8b9 20 #define II_CONSTANT 1
sahilmgandhi 19:7b66a518b6f8 21 #define ID_CONSTANT 3.1
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 19:7b66a518b6f8 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 19:7b66a518b6f8 225 left_motor.forward(0.11);
sahilmgandhi 16:d9252437bd92 226 right_motor.forward(0.10);
sahilmgandhi 19:7b66a518b6f8 227 wait_ms(1);
sahilmgandhi 16:d9252437bd92 228 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
sahilmgandhi 19:7b66a518b6f8 229 receiverTwoReading = IRP_2.getSamples(5);
sahilmgandhi 19:7b66a518b6f8 230 receiverThreeReading = IRP_3.getSamples(5);
sahilmgandhi 19:7b66a518b6f8 231 if (receiverThreeReading < 0.0007f)
sahilmgandhi 19:7b66a518b6f8 232 turnFlag |= RIGHT_FLAG;
sahilmgandhi 19:7b66a518b6f8 233 else if (receiverTwoReading < 0.0009f)
sahilmgandhi 17:f713758f6238 234 turnFlag |= LEFT_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 19:7b66a518b6f8 245 // moveForwardCellEncoder(0.3);
sahilmgandhi 17:f713758f6238 246 turnLeft();
sahilmgandhi 17:f713758f6238 247 }
sahilmgandhi 17:f713758f6238 248 else if (turnFlag == 0x2){
sahilmgandhi 19:7b66a518b6f8 249 // moveForwardCellEncoder(0.3);
sahilmgandhi 17:f713758f6238 250 turnRight();
sahilmgandhi 17:f713758f6238 251 }
sahilmgandhi 17:f713758f6238 252 else if (turnFlag == 0x3){
sahilmgandhi 19:7b66a518b6f8 253 // moveForwardCellEncoder(0.3);
sahilmgandhi 17:f713758f6238 254 turnLeft();
sahilmgandhi 19:7b66a518b6f8 255 turnRight();
sahilmgandhi 17:f713758f6238 256 }
sahilmgandhi 17:f713758f6238 257 }
sahilmgandhi 17:f713758f6238 258
sahilmgandhi 14:9e7bb03ddccb 259 void moveForwardUntilWallIr()
sahilmgandhi 14:9e7bb03ddccb 260 {
kyleliangus 9:1d8e4da058cd 261 double currentError = 0;
kyleliangus 9:1d8e4da058cd 262 double previousError = 0;
kyleliangus 9:1d8e4da058cd 263 double derivError = 0;
kyleliangus 9:1d8e4da058cd 264 double sumError = 0;
sahilmgandhi 14:9e7bb03ddccb 265
vanshg 11:8fc2b703086b 266 double HIGH_PWM_VOLTAGE = 0.1;
sahilmgandhi 14:9e7bb03ddccb 267
vanshg 11:8fc2b703086b 268 double rightSpeed = 0.1;
vanshg 11:8fc2b703086b 269 double leftSpeed = 0.1;
sahilmgandhi 14:9e7bb03ddccb 270
kyleliangus 9:1d8e4da058cd 271 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 272 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 14:9e7bb03ddccb 273
vanshg 11:8fc2b703086b 274 int count = encoder0.getPulses();
vanshg 11:8fc2b703086b 275 while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {
vanshg 11:8fc2b703086b 276 int pulseCount = (encoder0.getPulses()-count) % 5600;
vanshg 11:8fc2b703086b 277 if (pulseCount > 5400 && pulseCount < 5800) {
vanshg 11:8fc2b703086b 278 blueLed.write(0);
vanshg 11:8fc2b703086b 279 } else {
vanshg 11:8fc2b703086b 280 blueLed.write(1);
vanshg 11:8fc2b703086b 281 }
sahilmgandhi 14:9e7bb03ddccb 282
kyleliangus 9:1d8e4da058cd 283 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 284 derivError = currentError - previousError;
sahilmgandhi 14:9e7bb03ddccb 285 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
kyleliangus 9:1d8e4da058cd 286 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
sahilmgandhi 14:9e7bb03ddccb 287 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 288 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 14:9e7bb03ddccb 289 } else { // r is faster than L. speed up l, slow down r
sahilmgandhi 14:9e7bb03ddccb 290 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 14:9e7bb03ddccb 291 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 292 }
sahilmgandhi 14:9e7bb03ddccb 293
kyleliangus 9:1d8e4da058cd 294 if (leftSpeed > 0.30) leftSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 295 if (leftSpeed < 0) leftSpeed = 0;
kyleliangus 9:1d8e4da058cd 296 if (rightSpeed > 0.30) rightSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 297 if (rightSpeed < 0) rightSpeed = 0;
kyleliangus 9:1d8e4da058cd 298
sahilmgandhi 14:9e7bb03ddccb 299 right_motor.forward(rightSpeed);
kyleliangus 9:1d8e4da058cd 300 left_motor.forward(leftSpeed);
sahilmgandhi 14:9e7bb03ddccb 301
kyleliangus 9:1d8e4da058cd 302 previousError = currentError;
sahilmgandhi 14:9e7bb03ddccb 303
kyleliangus 9:1d8e4da058cd 304 ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 14:9e7bb03ddccb 305 ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 14:9e7bb03ddccb 306
kyleliangus 9:1d8e4da058cd 307 }
kyleliangus 9:1d8e4da058cd 308 //sensor1Turn = IR_Sensor1.read();
kyleliangus 9:1d8e4da058cd 309 //sensor4Turn = IR_Sensor4.read();
sahilmgandhi 14:9e7bb03ddccb 310
kyleliangus 9:1d8e4da058cd 311 //backward();
kyleliangus 9:1d8e4da058cd 312 wait_ms(40);
kyleliangus 9:1d8e4da058cd 313 //brake();
sahilmgandhi 14:9e7bb03ddccb 314
kyleliangus 9:1d8e4da058cd 315 left_motor.brake();
kyleliangus 9:1d8e4da058cd 316 right_motor.brake();
vanshg 10:810d1849da9d 317 }
vanshg 10:810d1849da9d 318
sahilmgandhi 14:9e7bb03ddccb 319 void printDipFlag()
sahilmgandhi 14:9e7bb03ddccb 320 {
vanshg 11:8fc2b703086b 321 if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 322 }
vanshg 11:8fc2b703086b 323
sahilmgandhi 14:9e7bb03ddccb 324 void enableButton1()
sahilmgandhi 14:9e7bb03ddccb 325 {
vanshg 10:810d1849da9d 326 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 327 printDipFlag();
vanshg 10:810d1849da9d 328 }
sahilmgandhi 14:9e7bb03ddccb 329 void enableButton2()
sahilmgandhi 14:9e7bb03ddccb 330 {
vanshg 10:810d1849da9d 331 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 332 printDipFlag();
vanshg 10:810d1849da9d 333 }
sahilmgandhi 14:9e7bb03ddccb 334 void enableButton3()
sahilmgandhi 14:9e7bb03ddccb 335 {
vanshg 10:810d1849da9d 336 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 337 printDipFlag();
vanshg 10:810d1849da9d 338 }
sahilmgandhi 14:9e7bb03ddccb 339 void enableButton4()
sahilmgandhi 14:9e7bb03ddccb 340 {
vanshg 10:810d1849da9d 341 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 342 printDipFlag();
vanshg 10:810d1849da9d 343 }
sahilmgandhi 14:9e7bb03ddccb 344 void disableButton1()
sahilmgandhi 14:9e7bb03ddccb 345 {
vanshg 10:810d1849da9d 346 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 347 printDipFlag();
vanshg 10:810d1849da9d 348 }
sahilmgandhi 14:9e7bb03ddccb 349 void disableButton2()
sahilmgandhi 14:9e7bb03ddccb 350 {
vanshg 10:810d1849da9d 351 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 352 printDipFlag();
vanshg 10:810d1849da9d 353 }
sahilmgandhi 14:9e7bb03ddccb 354 void disableButton3()
sahilmgandhi 14:9e7bb03ddccb 355 {
vanshg 10:810d1849da9d 356 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 357 printDipFlag();
vanshg 10:810d1849da9d 358 }
sahilmgandhi 14:9e7bb03ddccb 359 void disableButton4()
sahilmgandhi 14:9e7bb03ddccb 360 {
vanshg 10:810d1849da9d 361 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 362 printDipFlag();
kyleliangus 9:1d8e4da058cd 363 }
christine222 3:880f15be8c72 364
kyleliangus 15:b80555a4a8b9 365 void pidOnEncoders()
kyleliangus 15:b80555a4a8b9 366 {
kyleliangus 15:b80555a4a8b9 367 int count0;
kyleliangus 15:b80555a4a8b9 368 int count1;
kyleliangus 15:b80555a4a8b9 369 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 370 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 371 int diff = count0 - count1;
kyleliangus 15:b80555a4a8b9 372 double kp = 0.000075;
sahilmgandhi 16:d9252437bd92 373 double kd = 0.000135;
kyleliangus 15:b80555a4a8b9 374 int prev = 0;
sahilmgandhi 16:d9252437bd92 375
sahilmgandhi 16:d9252437bd92 376 int counter = 0;
kyleliangus 15:b80555a4a8b9 377 while(1)
kyleliangus 15:b80555a4a8b9 378 {
kyleliangus 15:b80555a4a8b9 379 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 380 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 381 int x = count0 - count1;
kyleliangus 15:b80555a4a8b9 382 //double d = kp * x + kd * ( x - prev );
kyleliangus 15:b80555a4a8b9 383 double kppart = kp * x;
kyleliangus 15:b80555a4a8b9 384 double kdpart = kd * (x-prev);
kyleliangus 15:b80555a4a8b9 385 double d = kppart + kdpart;
kyleliangus 15:b80555a4a8b9 386
kyleliangus 15:b80555a4a8b9 387 //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 388 if( x < diff - 50 ) // count1 is bigger, right wheel pushed forward
kyleliangus 15:b80555a4a8b9 389 {
kyleliangus 15:b80555a4a8b9 390 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 391 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 392 }
kyleliangus 15:b80555a4a8b9 393 else if( x > diff + 50 )
kyleliangus 15:b80555a4a8b9 394 {
kyleliangus 15:b80555a4a8b9 395 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 396 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 397 }
sahilmgandhi 16:d9252437bd92 398 // else
sahilmgandhi 16:d9252437bd92 399 // {
sahilmgandhi 16:d9252437bd92 400 // left_motor.brake();
sahilmgandhi 16:d9252437bd92 401 // right_motor.brake();
sahilmgandhi 16:d9252437bd92 402 // }
kyleliangus 15:b80555a4a8b9 403 prev = x;
sahilmgandhi 16:d9252437bd92 404 counter++;
sahilmgandhi 19:7b66a518b6f8 405 if (counter == 5)
sahilmgandhi 16:d9252437bd92 406 break;
kyleliangus 15:b80555a4a8b9 407 }
kyleliangus 15:b80555a4a8b9 408 }
kyleliangus 15:b80555a4a8b9 409
sahilmgandhi 17:f713758f6238 410 void oneCellEncoderAndIR(){
sahilmgandhi 19:7b66a518b6f8 411 double currentError = 0;
sahilmgandhi 19:7b66a518b6f8 412 double previousError = 0;
sahilmgandhi 19:7b66a518b6f8 413 double derivError = 0;
sahilmgandhi 19:7b66a518b6f8 414 double sumError = 0;
sahilmgandhi 17:f713758f6238 415
sahilmgandhi 19:7b66a518b6f8 416 double HIGH_PWM_VOLTAGE = 0.1;
sahilmgandhi 19:7b66a518b6f8 417 double rightSpeed = 0.10;
sahilmgandhi 19:7b66a518b6f8 418 double leftSpeed = 0.10;
sahilmgandhi 19:7b66a518b6f8 419
sahilmgandhi 19:7b66a518b6f8 420 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum;
sahilmgandhi 19:7b66a518b6f8 421 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum;
sahilmgandhi 19:7b66a518b6f8 422
sahilmgandhi 19:7b66a518b6f8 423 left_motor.forward(0.11);
sahilmgandhi 19:7b66a518b6f8 424 right_motor.forward(0.10);
sahilmgandhi 19:7b66a518b6f8 425
sahilmgandhi 19:7b66a518b6f8 426 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 427 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 428
sahilmgandhi 19:7b66a518b6f8 429 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
sahilmgandhi 19:7b66a518b6f8 430 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
sahilmgandhi 19:7b66a518b6f8 431 derivError = currentError - previousError;
sahilmgandhi 19:7b66a518b6f8 432 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
sahilmgandhi 19:7b66a518b6f8 433 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
sahilmgandhi 19:7b66a518b6f8 434 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 19:7b66a518b6f8 435 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 19:7b66a518b6f8 436 } else { // r is faster than L. speed up l, slow down r
sahilmgandhi 19:7b66a518b6f8 437 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 19:7b66a518b6f8 438 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 19:7b66a518b6f8 439 }
sahilmgandhi 19:7b66a518b6f8 440 if (leftSpeed > 0.30) leftSpeed = 0.30;
sahilmgandhi 19:7b66a518b6f8 441 if (leftSpeed < 0) leftSpeed = 0;
sahilmgandhi 19:7b66a518b6f8 442 if (rightSpeed > 0.30) rightSpeed = 0.30;
sahilmgandhi 19:7b66a518b6f8 443 if (rightSpeed < 0) rightSpeed = 0;
sahilmgandhi 19:7b66a518b6f8 444
sahilmgandhi 19:7b66a518b6f8 445 right_motor.forward(rightSpeed);
sahilmgandhi 19:7b66a518b6f8 446 left_motor.forward(leftSpeed);
sahilmgandhi 19:7b66a518b6f8 447 pidOnEncoders();
sahilmgandhi 19:7b66a518b6f8 448
sahilmgandhi 19:7b66a518b6f8 449 previousError = currentError;
sahilmgandhi 19:7b66a518b6f8 450 ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 451 ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 452 }
sahilmgandhi 19:7b66a518b6f8 453
sahilmgandhi 19:7b66a518b6f8 454 left_motor.brake();
sahilmgandhi 19:7b66a518b6f8 455 right_motor.brake();
sahilmgandhi 17:f713758f6238 456 }
sahilmgandhi 17:f713758f6238 457
sahilmgandhi 0:a03c771ab78e 458 int main()
sahilmgandhi 0:a03c771ab78e 459 {
christine222 3:880f15be8c72 460 //Set highest bandwidth.
sahilmgandhi 1:8a4b2f573923 461 gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 462 serial.baud(9600);
sahilmgandhi 7:6f5cb6377bd4 463 serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 7:6f5cb6377bd4 464
sahilmgandhi 1:8a4b2f573923 465 wait (0.1);
sahilmgandhi 7:6f5cb6377bd4 466
christine222 3:880f15be8c72 467
sahilmgandhi 2:771db996cee0 468 redLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 469 greenLed.write(0);
sahilmgandhi 2:771db996cee0 470 blueLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 471
kyleliangus 9:1d8e4da058cd 472 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 473 //right_motor.forward(0.1);
kyleliangus 8:a0760acdc59e 474
kyleliangus 8:a0760acdc59e 475 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 476 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 477 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 478 // PB_3 is B of left
vanshg 11:8fc2b703086b 479 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
vanshg 11:8fc2b703086b 480 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 14:9e7bb03ddccb 481
vanshg 10:810d1849da9d 482 // TODO: Setting all the registers and what not for Quadrature Encoders
sahilmgandhi 14:9e7bb03ddccb 483 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
sahilmgandhi 14:9e7bb03ddccb 484 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
sahilmgandhi 14:9e7bb03ddccb 485 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
sahilmgandhi 14:9e7bb03ddccb 486 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
sahilmgandhi 14:9e7bb03ddccb 487 */
sahilmgandhi 14:9e7bb03ddccb 488
kyleliangus 12:5790e56a056f 489 // 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 490 // of alternate function specified
kyleliangus 12:5790e56a056f 491 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 492 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 493 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 494 //
kyleliangus 12:5790e56a056f 495 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 496 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 497 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 498 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 499 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 500 // CR1 reenabales
kyleliangus 12:5790e56a056f 501 // then read CNT reg of timer
sahilmgandhi 14:9e7bb03ddccb 502
sahilmgandhi 14:9e7bb03ddccb 503
vanshg 10:810d1849da9d 504 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 505 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 506 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 507 dipButton4.rise(&enableButton4);
sahilmgandhi 14:9e7bb03ddccb 508
vanshg 10:810d1849da9d 509 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 510 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 511 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 512 dipButton4.fall(&disableButton4);
sahilmgandhi 7:6f5cb6377bd4 513
kyleliangus 15:b80555a4a8b9 514 //right_motor.forward( 0.2 );
kyleliangus 15:b80555a4a8b9 515 //left_motor.forward( 0.2 );
kyleliangus 15:b80555a4a8b9 516
christine222 3:880f15be8c72 517 while (1) {
sahilmgandhi 19:7b66a518b6f8 518 oneCellEncoderAndIR();
sahilmgandhi 19:7b66a518b6f8 519 // moveForwardCellEncoder(1);
sahilmgandhi 19:7b66a518b6f8 520 // wait(0.5);
sahilmgandhi 19:7b66a518b6f8 521 // handleTurns();
sahilmgandhi 19:7b66a518b6f8 522 // wait(0.5);
sahilmgandhi 19:7b66a518b6f8 523 // moveForwardCellEncoder(1);
sahilmgandhi 19:7b66a518b6f8 524 // wait(0.5);
sahilmgandhi 19:7b66a518b6f8 525 // handleTurns();
sahilmgandhi 17:f713758f6238 526 break;
sahilmgandhi 17:f713758f6238 527 // moveForwardOneCellEncoder();
sahilmgandhi 16:d9252437bd92 528 //pidOnEncoders();
kyleliangus 15:b80555a4a8b9 529 //moveForwardUntilWallIr();
kyleliangus 8:a0760acdc59e 530 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
kyleliangus 15:b80555a4a8b9 531 //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
sahilmgandhi 17:f713758f6238 532 // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
sahilmgandhi 17:f713758f6238 533 // serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",
sahilmgandhi 17:f713758f6238 534 // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
sahilmgandhi 14:9e7bb03ddccb 535
christine222 3:880f15be8c72 536 //reading = Rec_4.read();
christine222 3:880f15be8c72 537 // serial.printf("reading: %f\n", reading);
sahilmgandhi 1:8a4b2f573923 538 // redLed.write(0);
sahilmgandhi 1:8a4b2f573923 539 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 540 // redLed.write(1);
sahilmgandhi 1:8a4b2f573923 541 // greenLed.write(0);
sahilmgandhi 1:8a4b2f573923 542 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 543 // greenLed.write(1);
sahilmgandhi 1:8a4b2f573923 544 // blueLed.write(0);
sahilmgandhi 1:8a4b2f573923 545 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 546 // blueLed.write(1);
christine222 3:880f15be8c72 547 }
sahilmgandhi 0:a03c771ab78e 548 }