MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@9:1d8e4da058cd, 2017-05-06 (annotated)

Committer:
kyleliangus
Date:
Sat May 06 01:31:44 2017 +0000
Revision:
9:1d8e4da058cd
Parent:
8:a0760acdc59e
Child:
10:810d1849da9d
IR PID is now implemented

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 7:6f5cb6377bd4 9 #include "QEI.h"
sahilmgandhi 0:a03c771ab78e 10
kyleliangus 4:b5b7836ca2b0 11 // PID
kyleliangus 9:1d8e4da058cd 12 #define P_CONSTANT 0
kyleliangus 9:1d8e4da058cd 13 #define I_CONSTANT 0
kyleliangus 9:1d8e4da058cd 14 #define D_CONSTANT 0
kyleliangus 9:1d8e4da058cd 15
kyleliangus 9:1d8e4da058cd 16 #define IP_CONSTANT 6
kyleliangus 9:1d8e4da058cd 17 #define II_CONSTANT 0
kyleliangus 9:1d8e4da058cd 18 #define ID_CONSTANT 1
sahilmgandhi 0:a03c771ab78e 19
kyleliangus 8:a0760acdc59e 20 #include "QEI.h"
kyleliangus 8:a0760acdc59e 21 #define PULSES 880
kyleliangus 9:1d8e4da058cd 22 #define SAMPLE_NUM 100
kyleliangus 9:1d8e4da058cd 23
kyleliangus 9:1d8e4da058cd 24
kyleliangus 9:1d8e4da058cd 25 void moveForwardUntilWallIr() {
kyleliangus 9:1d8e4da058cd 26 double currentError = 0;
kyleliangus 9:1d8e4da058cd 27 double previousError = 0;
kyleliangus 9:1d8e4da058cd 28 double derivError = 0;
kyleliangus 9:1d8e4da058cd 29 double sumError = 0;
kyleliangus 9:1d8e4da058cd 30
kyleliangus 9:1d8e4da058cd 31 double HIGH_PWM_VOLTAGE = 0.2;
kyleliangus 9:1d8e4da058cd 32
kyleliangus 9:1d8e4da058cd 33 double rightSpeed = 0.2;
kyleliangus 9:1d8e4da058cd 34 double leftSpeed = 0.2;
kyleliangus 9:1d8e4da058cd 35
kyleliangus 9:1d8e4da058cd 36 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 37 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 38
kyleliangus 9:1d8e4da058cd 39 while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {
kyleliangus 9:1d8e4da058cd 40 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 41 derivError = currentError - previousError;
kyleliangus 9:1d8e4da058cd 42 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
kyleliangus 9:1d8e4da058cd 43 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
kyleliangus 9:1d8e4da058cd 44 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 45 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 46 } else { // r is faster than L. speed up l, slow down r
kyleliangus 9:1d8e4da058cd 47 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 48 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 49 }
kyleliangus 9:1d8e4da058cd 50
kyleliangus 9:1d8e4da058cd 51 if (leftSpeed > 0.30) leftSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 52 if (leftSpeed < 0) leftSpeed = 0;
kyleliangus 9:1d8e4da058cd 53 if (rightSpeed > 0.30) rightSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 54 if (rightSpeed < 0) rightSpeed = 0;
kyleliangus 9:1d8e4da058cd 55
kyleliangus 9:1d8e4da058cd 56 right_motor.forward(rightSpeed);
kyleliangus 9:1d8e4da058cd 57 left_motor.forward(leftSpeed);
kyleliangus 9:1d8e4da058cd 58
kyleliangus 9:1d8e4da058cd 59 previousError = currentError;
kyleliangus 9:1d8e4da058cd 60
kyleliangus 9:1d8e4da058cd 61 ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 62 ir3 = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 63
kyleliangus 9:1d8e4da058cd 64 }
kyleliangus 9:1d8e4da058cd 65 //sensor1Turn = IR_Sensor1.read();
kyleliangus 9:1d8e4da058cd 66 //sensor4Turn = IR_Sensor4.read();
kyleliangus 9:1d8e4da058cd 67
kyleliangus 9:1d8e4da058cd 68 //backward();
kyleliangus 9:1d8e4da058cd 69 wait_ms(40);
kyleliangus 9:1d8e4da058cd 70 //brake();
kyleliangus 9:1d8e4da058cd 71
kyleliangus 9:1d8e4da058cd 72 left_motor.brake();
kyleliangus 9:1d8e4da058cd 73 right_motor.brake();
kyleliangus 9:1d8e4da058cd 74 while( 1 )
kyleliangus 9:1d8e4da058cd 75 {
kyleliangus 9:1d8e4da058cd 76
kyleliangus 9:1d8e4da058cd 77 }
kyleliangus 9:1d8e4da058cd 78
kyleliangus 9:1d8e4da058cd 79
kyleliangus 9:1d8e4da058cd 80 }
christine222 3:880f15be8c72 81
sahilmgandhi 0:a03c771ab78e 82 int main()
sahilmgandhi 0:a03c771ab78e 83 {
kyleliangus 8:a0760acdc59e 84 //enableMotors();
christine222 3:880f15be8c72 85 //Set highest bandwidth.
sahilmgandhi 1:8a4b2f573923 86 gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 87 serial.baud(9600);
sahilmgandhi 7:6f5cb6377bd4 88 serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 7:6f5cb6377bd4 89
sahilmgandhi 1:8a4b2f573923 90 wait (0.1);
sahilmgandhi 7:6f5cb6377bd4 91
christine222 3:880f15be8c72 92 // IR_1.write(1);
sahilmgandhi 2:771db996cee0 93 // IR_2.write(1);
sahilmgandhi 2:771db996cee0 94 // IR_3.write(1);
sahilmgandhi 2:771db996cee0 95 // IR_4.write(1);
christine222 3:880f15be8c72 96
sahilmgandhi 2:771db996cee0 97 redLed.write(1);
sahilmgandhi 2:771db996cee0 98 greenLed.write(0);
sahilmgandhi 2:771db996cee0 99 blueLed.write(1);
sahilmgandhi 1:8a4b2f573923 100
kyleliangus 9:1d8e4da058cd 101 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 102 //right_motor.forward(0.1);
kyleliangus 8:a0760acdc59e 103
kyleliangus 8:a0760acdc59e 104 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 105 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 106 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 107 // PB_3 is B of left
kyleliangus 8:a0760acdc59e 108 QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
kyleliangus 8:a0760acdc59e 109 QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 7:6f5cb6377bd4 110
christine222 3:880f15be8c72 111 while (1) {
kyleliangus 9:1d8e4da058cd 112 moveForwardUntilWallIr();
kyleliangus 8:a0760acdc59e 113 wait(0.1);
kyleliangus 8:a0760acdc59e 114 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
kyleliangus 9:1d8e4da058cd 115 //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
kyleliangus 9:1d8e4da058cd 116 //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 117
kyleliangus 9:1d8e4da058cd 118 //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",
kyleliangus 9:1d8e4da058cd 119 // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
kyleliangus 9:1d8e4da058cd 120
christine222 3:880f15be8c72 121 //reading = Rec_4.read();
christine222 3:880f15be8c72 122 // serial.printf("reading: %f\n", reading);
sahilmgandhi 1:8a4b2f573923 123 // redLed.write(0);
sahilmgandhi 1:8a4b2f573923 124 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 125 // redLed.write(1);
sahilmgandhi 1:8a4b2f573923 126 // greenLed.write(0);
sahilmgandhi 1:8a4b2f573923 127 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 128 // greenLed.write(1);
sahilmgandhi 1:8a4b2f573923 129 // blueLed.write(0);
sahilmgandhi 1:8a4b2f573923 130 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 131 // blueLed.write(1);
christine222 3:880f15be8c72 132 }
sahilmgandhi 0:a03c771ab78e 133 }