Jamie Bignell / Mbed 2 deprecated MouseHybridSenseCode

This code enables our mouse to go round the track using the two error methods.

Dependencies:   Nucleo_blink_led mbed

Fork of Nucleo_blink_led by Jamie Bignell

main.cpp@6:a652deaae134, 2018-04-01 (annotated)

Committer:
JamieBignell
Date:
Sun Apr 01 18:21:50 2018 +0000
Revision:
6:a652deaae134
Parent:
5:dfea493f7a12
Child:
7:11dd5581c763
working version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JamieBignell 5:dfea493f7a12 1 #include <mbed.h>
JamieBignell 5:dfea493f7a12 2 #include <funcdef.h>
JamieBignell 5:dfea493f7a12 3 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
JamieBignell 5:dfea493f7a12 4 //Initialise Global Variables
JamieBignell 5:dfea493f7a12 5
JamieBignell 5:dfea493f7a12 6 int sensorValue[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 7
JamieBignell 5:dfea493f7a12 8 int sensorMinValue[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 9 int sensorMaxValue[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 10
JamieBignell 5:dfea493f7a12 11 int sensorMinValue1[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 12 int sensorMaxValue1[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 13
JamieBignell 5:dfea493f7a12 14 int sensorMinValue2[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 15 int sensorMaxValue2[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 16
JamieBignell 5:dfea493f7a12 17 int sensorThreshold[] = {0,0,0,0,0,0,0,0};
JamieBignell 6:a652deaae134 18 float sensorErrors[] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
JamieBignell 5:dfea493f7a12 19
JamieBignell 6:a652deaae134 20 float Prop=0;
JamieBignell 6:a652deaae134 21 float Inte=0;
JamieBignell 6:a652deaae134 22 float Dere=0;
JamieBignell 5:dfea493f7a12 23
JamieBignell 6:a652deaae134 24 float errorVar=0;
JamieBignell 6:a652deaae134 25 float previousError=0;
JamieBignell 6:a652deaae134 26 float PIDs=0;
JamieBignell 6:a652deaae134 27 int newPIDs=0;
JamieBignell 6:a652deaae134 28 int accumulator=0;
JamieBignell 6:a652deaae134 29 bool stopMotors=false;
JamieBignell 6:a652deaae134 30
JamieBignell 6:a652deaae134 31 int leftMotorValTemp=0;
JamieBignell 6:a652deaae134 32 int rightMotorValTemp=0;
JamieBignell 6:a652deaae134 33
JamieBignell 6:a652deaae134 34 int initialMotorspeed = 510;
JamieBignell 6:a652deaae134 35 Ticker ticker;
JamieBignell 5:dfea493f7a12 36
JamieBignell 5:dfea493f7a12 37 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
JamieBignell 5:dfea493f7a12 38 //Main Code
JamieBignell 0:c0c329729a7a 39
JamieBignell 6:a652deaae134 40 void tracker() {
JamieBignell 6:a652deaae134 41 //IF THE ERROR IS EQUAL TO THE LAST ERROR, START A COUNTING VARIABLE AND ADD THIS TO THE NOMINAL MOTOR SPEED (IE WHEN ERROR OF 0), IF CASE NOT TRUE, DO NOTHING/RETURN TO NORMAL SPEED FOR CORNER.
JamieBignell 6:a652deaae134 42 if (abs(PIDs)<=75)
JamieBignell 6:a652deaae134 43 {
JamieBignell 6:a652deaae134 44 accumulator++;
JamieBignell 6:a652deaae134 45 LEDVal=1;
JamieBignell 6:a652deaae134 46 }
JamieBignell 6:a652deaae134 47 else
JamieBignell 6:a652deaae134 48 {
JamieBignell 6:a652deaae134 49 accumulator=0;
JamieBignell 6:a652deaae134 50 LEDVal=0;
JamieBignell 6:a652deaae134 51 }
JamieBignell 6:a652deaae134 52
JamieBignell 6:a652deaae134 53 if (accumulator>=4)
JamieBignell 6:a652deaae134 54 {
JamieBignell 6:a652deaae134 55 initialMotorspeed = 650;
JamieBignell 6:a652deaae134 56 }
JamieBignell 6:a652deaae134 57 else if (accumulator>=2)
JamieBignell 6:a652deaae134 58 {
JamieBignell 6:a652deaae134 59 initialMotorspeed = 600;
JamieBignell 6:a652deaae134 60 }
JamieBignell 6:a652deaae134 61 else
JamieBignell 6:a652deaae134 62 {
JamieBignell 6:a652deaae134 63 initialMotorspeed = 510;
JamieBignell 6:a652deaae134 64 }
JamieBignell 6:a652deaae134 65 }
JamieBignell 6:a652deaae134 66
JamieBignell 3:7f44ab64e96b 67 int main()
JamieBignell 5:dfea493f7a12 68 {
JamieBignell 6:a652deaae134 69 //CONFIGURE MOTORS & TURN OFF LED
JamieBignell 4:f1670eec4681 70
JamieBignell 4:f1670eec4681 71 LEDVal=0;
JamieBignell 5:dfea493f7a12 72 leftMotorVal.period_us(1000);
JamieBignell 5:dfea493f7a12 73 rightMotorVal.period_us(1000);
JamieBignell 5:dfea493f7a12 74 leftMotorVal.pulsewidth_us(0);
JamieBignell 5:dfea493f7a12 75 rightMotorVal.pulsewidth_us(0);
JamieBignell 4:f1670eec4681 76
JamieBignell 5:dfea493f7a12 77 wait_ms(1000);
JamieBignell 4:f1670eec4681 78
JamieBignell 6:a652deaae134 79 //TURN LED ON
JamieBignell 6:a652deaae134 80 LEDVal=1;
JamieBignell 6:a652deaae134 81
JamieBignell 5:dfea493f7a12 82 //BEGIN CALIBRATION
JamieBignell 5:dfea493f7a12 83 CalibrateFunc();
JamieBignell 6:a652deaae134 84
JamieBignell 5:dfea493f7a12 85 //TURN OFF LED AND SET PULSES TO 2 HZ
JamieBignell 5:dfea493f7a12 86 LEDVal=0;
JamieBignell 6:a652deaae134 87
JamieBignell 5:dfea493f7a12 88 //FIND LINE
JamieBignell 6:a652deaae134 89 findLineFunc();
JamieBignell 4:f1670eec4681 90
JamieBignell 5:dfea493f7a12 91 //ONCE FOUND, HAND OVER TO MAIN ALGORITHM
JamieBignell 6:a652deaae134 92 ticker.attach(&tracker, 0.1);
JamieBignell 5:dfea493f7a12 93 while(1)
JamieBignell 5:dfea493f7a12 94 {
JamieBignell 5:dfea493f7a12 95 senseFunc();
JamieBignell 6:a652deaae134 96 CountingVar=!CountingVar;
JamieBignell 5:dfea493f7a12 97 calculateFunc();
JamieBignell 5:dfea493f7a12 98 driveFunc();
JamieBignell 4:f1670eec4681 99 }
JamieBignell 6:a652deaae134 100
JamieBignell 3:7f44ab64e96b 101 return 0;
JamieBignell 3:7f44ab64e96b 102 }