E=MC / Mbed 2 deprecated linecam_practice

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@4:a8dce9e269e5, 2015-03-12 (annotated)

Committer:
mawk2311
Date:
Thu Mar 12 19:25:23 2015 +0000
Revision:
4:a8dce9e269e5
Parent:
3:4ba3d22e50dc
Child:
5:60560dd127a2
Basic Line Tracking is there. We can track the edges of the track, but once we only have one edge of the track, we lose it.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ericoneill 0:a7a8c6ef6d11 1 #include "mbed.h"
mawk2311 4:a8dce9e269e5 2 #include "stdlib.h"
ericoneill 0:a7a8c6ef6d11 3
ericoneill 0:a7a8c6ef6d11 4 DigitalOut clk(PTA13);
ericoneill 1:c6fa316ce7d1 5 DigitalOut si(PTD4);
ericoneill 0:a7a8c6ef6d11 6 AnalogIn camData(PTC2);
mawk2311 3:4ba3d22e50dc 7 PwmOut servo(PTA5);
mawk2311 3:4ba3d22e50dc 8 Serial pc(USBTX, USBRX); // tx, rx
mawk2311 3:4ba3d22e50dc 9
mawk2311 3:4ba3d22e50dc 10 float ADCdata [128];
mawk2311 3:4ba3d22e50dc 11 float slopeAccum;
mawk2311 3:4ba3d22e50dc 12 float slopeCount;
mawk2311 3:4ba3d22e50dc 13 float approxPos;
mawk2311 3:4ba3d22e50dc 14 float minVal;
mawk2311 3:4ba3d22e50dc 15
mawk2311 3:4ba3d22e50dc 16 int peak1;
mawk2311 3:4ba3d22e50dc 17 int peak2;
mawk2311 3:4ba3d22e50dc 18
mawk2311 4:a8dce9e269e5 19 int peakLoc1;
mawk2311 4:a8dce9e269e5 20 int peakLoc2;
mawk2311 4:a8dce9e269e5 21
mawk2311 4:a8dce9e269e5 22 int risEdge;
mawk2311 4:a8dce9e269e5 23
mawk2311 3:4ba3d22e50dc 24 float currSlope;
mawk2311 3:4ba3d22e50dc 25
mawk2311 3:4ba3d22e50dc 26 float straight = 0.00155f;
mawk2311 3:4ba3d22e50dc 27 float hardLeft = 0.0013f;
mawk2311 3:4ba3d22e50dc 28 float slightLeft = 0.00145f;
mawk2311 3:4ba3d22e50dc 29 float hardRight = 0.0018f;
mawk2311 3:4ba3d22e50dc 30 float slightRight = 0.00165f;
mawk2311 3:4ba3d22e50dc 31
mawk2311 4:a8dce9e269e5 32 float currDirection = straight;
mawk2311 4:a8dce9e269e5 33
mawk2311 4:a8dce9e269e5 34 int comp (const void * elem1, const void * elem2)
mawk2311 4:a8dce9e269e5 35 {
mawk2311 4:a8dce9e269e5 36 int f = *((int*)elem1);
mawk2311 4:a8dce9e269e5 37 int s = *((int*)elem2);
mawk2311 4:a8dce9e269e5 38 if (f > s) return 1;
mawk2311 4:a8dce9e269e5 39 if (f < s) return -1;
mawk2311 4:a8dce9e269e5 40 return 0;
mawk2311 4:a8dce9e269e5 41 }
mawk2311 4:a8dce9e269e5 42
mawk2311 4:a8dce9e269e5 43
mawk2311 4:a8dce9e269e5 44
ericoneill 0:a7a8c6ef6d11 45 int main() {
mawk2311 4:a8dce9e269e5 46 //servo.period(SERVO_FREQ);
ericoneill 1:c6fa316ce7d1 47 int integrationCounter = 0;
mawk2311 3:4ba3d22e50dc 48
ericoneill 0:a7a8c6ef6d11 49 while(1) {
mawk2311 3:4ba3d22e50dc 50
mawk2311 4:a8dce9e269e5 51 if(integrationCounter % 151== 0){
ericoneill 1:c6fa316ce7d1 52 si = 1;
ericoneill 2:f3eafd4d3705 53 clk = 1;
ericoneill 2:f3eafd4d3705 54 //wait(.00001);
ericoneill 1:c6fa316ce7d1 55 si = 0;
ericoneill 2:f3eafd4d3705 56 clk = 0;
mawk2311 3:4ba3d22e50dc 57 integrationCounter = 0;
mawk2311 3:4ba3d22e50dc 58
mawk2311 3:4ba3d22e50dc 59 slopeAccum = 0;
mawk2311 3:4ba3d22e50dc 60 slopeCount = 0;
mawk2311 3:4ba3d22e50dc 61 approxPos = 0;
mawk2311 3:4ba3d22e50dc 62 peak1 = 0;
mawk2311 3:4ba3d22e50dc 63 peak2 = 0;
mawk2311 4:a8dce9e269e5 64 risEdge = 0;
mawk2311 4:a8dce9e269e5 65 peakLoc1 = 0;
mawk2311 4:a8dce9e269e5 66 peakLoc2 = 0;
mawk2311 3:4ba3d22e50dc 67
mawk2311 3:4ba3d22e50dc 68 }
mawk2311 3:4ba3d22e50dc 69 else if (integrationCounter > 129){
mawk2311 4:a8dce9e269e5 70 minVal = 0;
mawk2311 4:a8dce9e269e5 71 for (int c = 0; c < 126; c++) {
mawk2311 4:a8dce9e269e5 72 if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && !peak1 && !risEdge && !peak2){
mawk2311 3:4ba3d22e50dc 73 peak1 = 1;
mawk2311 4:a8dce9e269e5 74 peakLoc1 = c;
mawk2311 3:4ba3d22e50dc 75 minVal = ADCdata[c];
mawk2311 4:a8dce9e269e5 76 } else if (ADCdata[c] < minVal && peak1 && !peak2){
mawk2311 3:4ba3d22e50dc 77 minVal = ADCdata[c];
mawk2311 4:a8dce9e269e5 78 } else if (ADCdata[c+1] > ADCdata[c] && ADCdata[c+2] > ADCdata[c+1] && peak1 && !peak2){
mawk2311 4:a8dce9e269e5 79 risEdge = 1;
mawk2311 4:a8dce9e269e5 80 } else if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && peak1 && risEdge && !peak2){
mawk2311 3:4ba3d22e50dc 81 peak2 = 1;
mawk2311 4:a8dce9e269e5 82 peakLoc2 = c;
mawk2311 3:4ba3d22e50dc 83 }
mawk2311 3:4ba3d22e50dc 84 }
mawk2311 4:a8dce9e269e5 85
mawk2311 4:a8dce9e269e5 86 for (int c = peakLoc1; c < peakLoc2; c++) {
mawk2311 4:a8dce9e269e5 87 if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.01f && ADCdata[c] > 0.1f){
mawk2311 3:4ba3d22e50dc 88 slopeAccum += c;
mawk2311 3:4ba3d22e50dc 89 slopeCount++;
mawk2311 3:4ba3d22e50dc 90 }
mawk2311 3:4ba3d22e50dc 91 }
mawk2311 4:a8dce9e269e5 92
mawk2311 3:4ba3d22e50dc 93 approxPos = (float)slopeAccum/(float)slopeCount;
mawk2311 3:4ba3d22e50dc 94
mawk2311 4:a8dce9e269e5 95 if (peak1 && peak2){
mawk2311 4:a8dce9e269e5 96 if(approxPos > 0 && approxPos <= 32){
mawk2311 4:a8dce9e269e5 97 if (currDirection != slightLeft){
mawk2311 4:a8dce9e269e5 98 currDirection = slightLeft;
mawk2311 4:a8dce9e269e5 99 servo.pulsewidth(slightLeft);
mawk2311 4:a8dce9e269e5 100 }
mawk2311 4:a8dce9e269e5 101 } else if (approxPos > 32 && approxPos <= 60){
mawk2311 4:a8dce9e269e5 102 if (currDirection != straight){
mawk2311 4:a8dce9e269e5 103 currDirection = straight;
mawk2311 4:a8dce9e269e5 104 servo.pulsewidth(straight);
mawk2311 4:a8dce9e269e5 105 }
mawk2311 4:a8dce9e269e5 106 } else if (approxPos > 70 && approxPos <= 128){
mawk2311 4:a8dce9e269e5 107 if (currDirection != slightRight){
mawk2311 4:a8dce9e269e5 108 currDirection = slightRight;
mawk2311 4:a8dce9e269e5 109 servo.pulsewidth(slightRight);
mawk2311 4:a8dce9e269e5 110 }
mawk2311 4:a8dce9e269e5 111 } else if (approxPos < 32){
mawk2311 4:a8dce9e269e5 112 if (currDirection != hardLeft){
mawk2311 4:a8dce9e269e5 113 currDirection = hardLeft;
mawk2311 4:a8dce9e269e5 114 servo.pulsewidth(hardLeft);
mawk2311 4:a8dce9e269e5 115 }
mawk2311 3:4ba3d22e50dc 116 }
mawk2311 4:a8dce9e269e5 117 }
mawk2311 4:a8dce9e269e5 118 integrationCounter = 150;
ericoneill 0:a7a8c6ef6d11 119 }
ericoneill 2:f3eafd4d3705 120 else{
ericoneill 2:f3eafd4d3705 121 clk = 1;
mawk2311 4:a8dce9e269e5 122 wait_us(50);
mawk2311 3:4ba3d22e50dc 123 ADCdata[integrationCounter - 1] = camData;
ericoneill 2:f3eafd4d3705 124 clk = 0;
ericoneill 2:f3eafd4d3705 125 }
ericoneill 2:f3eafd4d3705 126
ericoneill 2:f3eafd4d3705 127 //clk = 0;
ericoneill 1:c6fa316ce7d1 128 integrationCounter++;
ericoneill 1:c6fa316ce7d1 129 //camData.
ericoneill 0:a7a8c6ef6d11 130
ericoneill 0:a7a8c6ef6d11 131 }
ericoneill 0:a7a8c6ef6d11 132 }