MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@11:8fc2b703086b, 2017-05-07 (annotated)

Committer:
vanshg
Date:
Sun May 07 01:13:42 2017 +0000
Revision:
11:8fc2b703086b
Parent:
10:810d1849da9d
Child:
12:5790e56a056f
Counting cells somewhat properly now

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"
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
vanshg 11:8fc2b703086b 20 #define IP_CONSTANT 15
vanshg 11:8fc2b703086b 21 #define II_CONSTANT 0
vanshg 11:8fc2b703086b 22 #define ID_CONSTANT 0
kyleliangus 9:1d8e4da058cd 23
kyleliangus 9:1d8e4da058cd 24 void moveForwardUntilWallIr() {
kyleliangus 9:1d8e4da058cd 25 double currentError = 0;
kyleliangus 9:1d8e4da058cd 26 double previousError = 0;
kyleliangus 9:1d8e4da058cd 27 double derivError = 0;
kyleliangus 9:1d8e4da058cd 28 double sumError = 0;
kyleliangus 9:1d8e4da058cd 29
vanshg 11:8fc2b703086b 30 double HIGH_PWM_VOLTAGE = 0.1;
kyleliangus 9:1d8e4da058cd 31
vanshg 11:8fc2b703086b 32 double rightSpeed = 0.1;
vanshg 11:8fc2b703086b 33 double leftSpeed = 0.1;
kyleliangus 9:1d8e4da058cd 34
kyleliangus 9:1d8e4da058cd 35 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 36 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 37
vanshg 11:8fc2b703086b 38 int count = encoder0.getPulses();
vanshg 11:8fc2b703086b 39 while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {
vanshg 11:8fc2b703086b 40 int pulseCount = (encoder0.getPulses()-count) % 5600;
vanshg 11:8fc2b703086b 41 if (pulseCount > 5400 && pulseCount < 5800) {
vanshg 11:8fc2b703086b 42 blueLed.write(0);
vanshg 11:8fc2b703086b 43 } else {
vanshg 11:8fc2b703086b 44 blueLed.write(1);
vanshg 11:8fc2b703086b 45 }
vanshg 11:8fc2b703086b 46
vanshg 11:8fc2b703086b 47
kyleliangus 9:1d8e4da058cd 48 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 49 derivError = currentError - previousError;
kyleliangus 9:1d8e4da058cd 50 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
kyleliangus 9:1d8e4da058cd 51 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
kyleliangus 9:1d8e4da058cd 52 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 53 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 54 } else { // r is faster than L. speed up l, slow down r
kyleliangus 9:1d8e4da058cd 55 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 56 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 57 }
kyleliangus 9:1d8e4da058cd 58
kyleliangus 9:1d8e4da058cd 59 if (leftSpeed > 0.30) leftSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 60 if (leftSpeed < 0) leftSpeed = 0;
kyleliangus 9:1d8e4da058cd 61 if (rightSpeed > 0.30) rightSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 62 if (rightSpeed < 0) rightSpeed = 0;
kyleliangus 9:1d8e4da058cd 63
kyleliangus 9:1d8e4da058cd 64 right_motor.forward(rightSpeed);
kyleliangus 9:1d8e4da058cd 65 left_motor.forward(leftSpeed);
kyleliangus 9:1d8e4da058cd 66
kyleliangus 9:1d8e4da058cd 67 previousError = currentError;
kyleliangus 9:1d8e4da058cd 68
kyleliangus 9:1d8e4da058cd 69 ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 70 ir3 = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 71
kyleliangus 9:1d8e4da058cd 72 }
kyleliangus 9:1d8e4da058cd 73 //sensor1Turn = IR_Sensor1.read();
kyleliangus 9:1d8e4da058cd 74 //sensor4Turn = IR_Sensor4.read();
kyleliangus 9:1d8e4da058cd 75
kyleliangus 9:1d8e4da058cd 76 //backward();
kyleliangus 9:1d8e4da058cd 77 wait_ms(40);
kyleliangus 9:1d8e4da058cd 78 //brake();
kyleliangus 9:1d8e4da058cd 79
kyleliangus 9:1d8e4da058cd 80 left_motor.brake();
kyleliangus 9:1d8e4da058cd 81 right_motor.brake();
kyleliangus 9:1d8e4da058cd 82 while( 1 )
kyleliangus 9:1d8e4da058cd 83 {
kyleliangus 9:1d8e4da058cd 84
kyleliangus 9:1d8e4da058cd 85 }
vanshg 10:810d1849da9d 86 }
vanshg 10:810d1849da9d 87
vanshg 11:8fc2b703086b 88 void printDipFlag() {
vanshg 11:8fc2b703086b 89 if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 90 }
vanshg 11:8fc2b703086b 91
vanshg 10:810d1849da9d 92 void enableButton1() {
vanshg 10:810d1849da9d 93 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 94 printDipFlag();
vanshg 10:810d1849da9d 95 }
vanshg 10:810d1849da9d 96 void enableButton2() {
vanshg 10:810d1849da9d 97 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 98 printDipFlag();
vanshg 10:810d1849da9d 99 }
vanshg 10:810d1849da9d 100 void enableButton3() {
vanshg 10:810d1849da9d 101 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 102 printDipFlag();
vanshg 10:810d1849da9d 103 }
vanshg 10:810d1849da9d 104 void enableButton4() {
vanshg 10:810d1849da9d 105 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 106 printDipFlag();
vanshg 10:810d1849da9d 107 }
vanshg 10:810d1849da9d 108 void disableButton1() {
vanshg 10:810d1849da9d 109 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 110 printDipFlag();
vanshg 10:810d1849da9d 111 }
vanshg 10:810d1849da9d 112 void disableButton2() {
vanshg 10:810d1849da9d 113 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 114 printDipFlag();
vanshg 10:810d1849da9d 115 }
vanshg 10:810d1849da9d 116 void disableButton3() {
vanshg 10:810d1849da9d 117 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 118 printDipFlag();
vanshg 10:810d1849da9d 119 }
vanshg 10:810d1849da9d 120 void disableButton4() {
vanshg 10:810d1849da9d 121 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 122 printDipFlag();
kyleliangus 9:1d8e4da058cd 123 }
christine222 3:880f15be8c72 124
sahilmgandhi 0:a03c771ab78e 125 int main()
sahilmgandhi 0:a03c771ab78e 126 {
kyleliangus 8:a0760acdc59e 127 //enableMotors();
christine222 3:880f15be8c72 128 //Set highest bandwidth.
sahilmgandhi 1:8a4b2f573923 129 gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 130 serial.baud(9600);
sahilmgandhi 7:6f5cb6377bd4 131 serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 7:6f5cb6377bd4 132
sahilmgandhi 1:8a4b2f573923 133 wait (0.1);
sahilmgandhi 7:6f5cb6377bd4 134
christine222 3:880f15be8c72 135 // IR_1.write(1);
sahilmgandhi 2:771db996cee0 136 // IR_2.write(1);
sahilmgandhi 2:771db996cee0 137 // IR_3.write(1);
sahilmgandhi 2:771db996cee0 138 // IR_4.write(1);
christine222 3:880f15be8c72 139
sahilmgandhi 2:771db996cee0 140 redLed.write(1);
vanshg 11:8fc2b703086b 141 greenLed.write(1);
sahilmgandhi 2:771db996cee0 142 blueLed.write(1);
sahilmgandhi 1:8a4b2f573923 143
kyleliangus 9:1d8e4da058cd 144 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 145 //right_motor.forward(0.1);
kyleliangus 8:a0760acdc59e 146
kyleliangus 8:a0760acdc59e 147 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 148 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 149 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 150 // PB_3 is B of left
vanshg 11:8fc2b703086b 151 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
vanshg 11:8fc2b703086b 152 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
vanshg 10:810d1849da9d 153
vanshg 10:810d1849da9d 154 // TODO: Setting all the registers and what not for Quadrature Encoders
vanshg 10:810d1849da9d 155 RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
vanshg 10:810d1849da9d 156 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
vanshg 10:810d1849da9d 157 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
vanshg 10:810d1849da9d 158 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
vanshg 10:810d1849da9d 159
vanshg 10:810d1849da9d 160 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 161 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 162 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 163 dipButton4.rise(&enableButton4);
vanshg 10:810d1849da9d 164
vanshg 10:810d1849da9d 165 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 166 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 167 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 168 dipButton4.fall(&disableButton4);
vanshg 10:810d1849da9d 169
sahilmgandhi 7:6f5cb6377bd4 170
christine222 3:880f15be8c72 171 while (1) {
vanshg 11:8fc2b703086b 172
kyleliangus 9:1d8e4da058cd 173 moveForwardUntilWallIr();
kyleliangus 8:a0760acdc59e 174 wait(0.1);
kyleliangus 8:a0760acdc59e 175 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
vanshg 11:8fc2b703086b 176 // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
kyleliangus 9:1d8e4da058cd 177 //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 178
kyleliangus 9:1d8e4da058cd 179 //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",
kyleliangus 9:1d8e4da058cd 180 // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
kyleliangus 9:1d8e4da058cd 181
christine222 3:880f15be8c72 182 //reading = Rec_4.read();
christine222 3:880f15be8c72 183 // serial.printf("reading: %f\n", reading);
sahilmgandhi 1:8a4b2f573923 184 // redLed.write(0);
sahilmgandhi 1:8a4b2f573923 185 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 186 // redLed.write(1);
sahilmgandhi 1:8a4b2f573923 187 // greenLed.write(0);
sahilmgandhi 1:8a4b2f573923 188 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 189 // greenLed.write(1);
sahilmgandhi 1:8a4b2f573923 190 // blueLed.write(0);
sahilmgandhi 1:8a4b2f573923 191 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 192 // blueLed.write(1);
christine222 3:880f15be8c72 193 }
sahilmgandhi 0:a03c771ab78e 194 }