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

calibrateFunc.cpp@7:11dd5581c763, 2018-04-28 (annotated)

Committer:
JamieBignell
Date:
Sat Apr 28 21:34:55 2018 +0000
Revision:
7:11dd5581c763
Parent:
6:a652deaae134 
Hybrid Mouse Code

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 3:7f44ab64e96b 3
JamieBignell 3:7f44ab64e96b 4 void CalibrateFunc()
JamieBignell 3:7f44ab64e96b 5 {
JamieBignell 3:7f44ab64e96b 6 //USER PUTS MOUSE LEFT OF LINE
JamieBignell 3:7f44ab64e96b 7
JamieBignell 3:7f44ab64e96b 8 //TAKE MIN/MAX VALUES 1ST TIME
JamieBignell 3:7f44ab64e96b 9 for (int i=0;i<=7;i++)
JamieBignell 3:7f44ab64e96b 10 {
JamieBignell 3:7f44ab64e96b 11 sensorValue[i]=((sensorPin[i]).read_u16()>>4);
JamieBignell 3:7f44ab64e96b 12 sensorMinValue1[i]=sensorValue[i];
JamieBignell 3:7f44ab64e96b 13 sensorMaxValue1[i]=sensorValue[i];
JamieBignell 3:7f44ab64e96b 14 }
JamieBignell 3:7f44ab64e96b 15
JamieBignell 5:dfea493f7a12 16 //LEFT WHEEL STARTS TURN
JamieBignell 6:a652deaae134 17 leftMotorVal.pulsewidth_us(calibrationMotorspeed);
JamieBignell 5:dfea493f7a12 18 rightMotorVal.pulsewidth_us(0);
JamieBignell 3:7f44ab64e96b 19
JamieBignell 3:7f44ab64e96b 20 //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET AND A SMIDGE MORE
JamieBignell 5:dfea493f7a12 21 /////////////////////////////////////////////
JamieBignell 5:dfea493f7a12 22 int doneYet[] = {0,0,0,0,0,0,0,0};
JamieBignell 3:7f44ab64e96b 23 while(1)
JamieBignell 3:7f44ab64e96b 24 {
JamieBignell 3:7f44ab64e96b 25 for (int i=0;i<=7;i++)
JamieBignell 3:7f44ab64e96b 26 {
JamieBignell 3:7f44ab64e96b 27 sensorValue[i]=((sensorPin[i]).read_u16()>>4);
JamieBignell 3:7f44ab64e96b 28 if (sensorValue[i]>=sensorMaxValue1[i])
JamieBignell 3:7f44ab64e96b 29 {
JamieBignell 3:7f44ab64e96b 30 sensorMaxValue1[i]=sensorValue[i];
JamieBignell 3:7f44ab64e96b 31 }
JamieBignell 5:dfea493f7a12 32 else if ((sensorValue[i]+250)<=sensorMaxValue1[i])
JamieBignell 3:7f44ab64e96b 33 {
JamieBignell 5:dfea493f7a12 34 doneYet[i]=1;
JamieBignell 5:dfea493f7a12 35 if (((doneYet[6]) == 1) && ((doneYet[4]) == 1) && ((doneYet[2]) == 1) && ((doneYet[0]) == 1) && ((doneYet[1]) == 1) && ((doneYet[3]) == 1) && ((doneYet[5]) == 1) && ((doneYet[7]) == 1))
JamieBignell 3:7f44ab64e96b 36 {
JamieBignell 5:dfea493f7a12 37 goto endOfScan1;
JamieBignell 3:7f44ab64e96b 38 }
JamieBignell 3:7f44ab64e96b 39 }
JamieBignell 3:7f44ab64e96b 40 }
JamieBignell 3:7f44ab64e96b 41 }
JamieBignell 3:7f44ab64e96b 42 endOfScan1:
JamieBignell 5:dfea493f7a12 43 wait_ms(50);
JamieBignell 3:7f44ab64e96b 44 /////////////////////////////////////////////
JamieBignell 3:7f44ab64e96b 45
JamieBignell 3:7f44ab64e96b 46 //STOP WHEELS
JamieBignell 5:dfea493f7a12 47 leftMotorVal.pulsewidth_us(0);
JamieBignell 5:dfea493f7a12 48 rightMotorVal.pulsewidth_us(0);
JamieBignell 5:dfea493f7a12 49
JamieBignell 6:a652deaae134 50 //TURN LED OFF
JamieBignell 5:dfea493f7a12 51 LEDVal=0;
JamieBignell 5:dfea493f7a12 52
JamieBignell 5:dfea493f7a12 53 wait_ms(500);
JamieBignell 3:7f44ab64e96b 54
JamieBignell 3:7f44ab64e96b 55 //TAKE MIN/MAX VALUES 2ND TIME
JamieBignell 3:7f44ab64e96b 56 for (int i=0;i<=7;i++)
JamieBignell 3:7f44ab64e96b 57 {
JamieBignell 3:7f44ab64e96b 58 sensorValue[i]=((sensorPin[i]).read_u16()>>4);
JamieBignell 3:7f44ab64e96b 59 sensorMinValue2[i]=sensorValue[i];
JamieBignell 3:7f44ab64e96b 60 sensorMaxValue2[i]=sensorValue[i];
JamieBignell 3:7f44ab64e96b 61 }
JamieBignell 3:7f44ab64e96b 62
JamieBignell 3:7f44ab64e96b 63 //RIGHT WHEEL STARTS TURN
JamieBignell 5:dfea493f7a12 64 leftMotorVal.pulsewidth_us(0);
JamieBignell 6:a652deaae134 65 rightMotorVal.pulsewidth_us(calibrationMotorspeed);
JamieBignell 3:7f44ab64e96b 66
JamieBignell 3:7f44ab64e96b 67 //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET IN NEW VARIABLES
JamieBignell 5:dfea493f7a12 68 int doneYet2[] = {0,0,0,0,0,0,0,0};
JamieBignell 3:7f44ab64e96b 69 while(1)
JamieBignell 3:7f44ab64e96b 70 {
JamieBignell 3:7f44ab64e96b 71 for (int i=0;i<=7;i++)
JamieBignell 3:7f44ab64e96b 72 {
JamieBignell 3:7f44ab64e96b 73 sensorValue[i]=((sensorPin[i]).read_u16()>>4);
JamieBignell 3:7f44ab64e96b 74 if (sensorValue[i]>=sensorMaxValue2[i])
JamieBignell 3:7f44ab64e96b 75 {
JamieBignell 3:7f44ab64e96b 76 sensorMaxValue2[i]=sensorValue[i];
JamieBignell 3:7f44ab64e96b 77 }
JamieBignell 5:dfea493f7a12 78 else if ((sensorValue[i]+250)<=sensorMaxValue2[i])
JamieBignell 5:dfea493f7a12 79 {
JamieBignell 5:dfea493f7a12 80 doneYet2[i]=1;
JamieBignell 5:dfea493f7a12 81 if (((doneYet2[6]) == 1) && ((doneYet2[4]) == 1) && ((doneYet2[2]) == 1) && ((doneYet2[0]) == 1) && ((doneYet2[1]) == 1) && ((doneYet2[3]) == 1) && ((doneYet2[5]) == 1) && ((doneYet2[7]) == 1))
JamieBignell 3:7f44ab64e96b 82 {
JamieBignell 3:7f44ab64e96b 83 goto endOfScan2;
JamieBignell 3:7f44ab64e96b 84 }
JamieBignell 3:7f44ab64e96b 85 }
JamieBignell 3:7f44ab64e96b 86 }
JamieBignell 3:7f44ab64e96b 87 }
JamieBignell 3:7f44ab64e96b 88 endOfScan2:
JamieBignell 3:7f44ab64e96b 89
JamieBignell 6:a652deaae134 90 wait_ms(50);
JamieBignell 3:7f44ab64e96b 91 /////////////////////////////////////////////
JamieBignell 3:7f44ab64e96b 92
JamieBignell 3:7f44ab64e96b 93 //STOP WHEELS
JamieBignell 5:dfea493f7a12 94 leftMotorVal.pulsewidth_us(0);
JamieBignell 5:dfea493f7a12 95 rightMotorVal.pulsewidth_us(0);
JamieBignell 5:dfea493f7a12 96
JamieBignell 6:a652deaae134 97 wait_ms(250);
JamieBignell 3:7f44ab64e96b 98
JamieBignell 3:7f44ab64e96b 99 //AVERAGE THESE VALUES
JamieBignell 5:dfea493f7a12 100 for (int i=0;i<=7;i++)
JamieBignell 3:7f44ab64e96b 101 {
JamieBignell 3:7f44ab64e96b 102 sensorMinValue[i]=(sensorMinValue1[i]+sensorMinValue2[i])/2;
JamieBignell 3:7f44ab64e96b 103 sensorMaxValue[i]=(sensorMaxValue1[i]+sensorMaxValue2[i])/2;
JamieBignell 3:7f44ab64e96b 104 }
JamieBignell 3:7f44ab64e96b 105
JamieBignell 3:7f44ab64e96b 106 //SET THRESHOLDS FOR ERROR OF 0
JamieBignell 5:dfea493f7a12 107 for (int i=0;i<=7;i++)
JamieBignell 3:7f44ab64e96b 108 {
JamieBignell 5:dfea493f7a12 109 sensorThreshold[i]=sensorMaxValue[i]-250;
JamieBignell 3:7f44ab64e96b 110 }
JamieBignell 5:dfea493f7a12 111 /////////////////////////////////////////////////////////////////////////////////////////////////////////////
JamieBignell 3:7f44ab64e96b 112 return;
JamieBignell 3:7f44ab64e96b 113 }