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@5:60560dd127a2, 2015-03-12 (annotated)

Committer:
mawk2311
Date:
Thu Mar 12 19:47:11 2015 +0000
Revision:
5:60560dd127a2
Parent:
4:a8dce9e269e5
Child:
6:53d521496469
Kickass. This seems to work pretty well. Just added a comparison of the intensities of each pick to determine whether we want to turn hard or slight in a given direction. Difference of these magnitudes seems to be different for each direction though.

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 5:60560dd127a2 16 float peakVal1;
mawk2311 5:60560dd127a2 17 float peakVal2;
mawk2311 5:60560dd127a2 18
mawk2311 3:4ba3d22e50dc 19 int peak1;
mawk2311 3:4ba3d22e50dc 20 int peak2;
mawk2311 3:4ba3d22e50dc 21
mawk2311 4:a8dce9e269e5 22 int peakLoc1;
mawk2311 4:a8dce9e269e5 23 int peakLoc2;
mawk2311 4:a8dce9e269e5 24
mawk2311 4:a8dce9e269e5 25 int risEdge;
mawk2311 4:a8dce9e269e5 26
mawk2311 3:4ba3d22e50dc 27 float currSlope;
mawk2311 3:4ba3d22e50dc 28
mawk2311 3:4ba3d22e50dc 29 float straight = 0.00155f;
mawk2311 3:4ba3d22e50dc 30 float hardLeft = 0.0013f;
mawk2311 3:4ba3d22e50dc 31 float slightLeft = 0.00145f;
mawk2311 3:4ba3d22e50dc 32 float hardRight = 0.0018f;
mawk2311 3:4ba3d22e50dc 33 float slightRight = 0.00165f;
mawk2311 3:4ba3d22e50dc 34
mawk2311 4:a8dce9e269e5 35 float currDirection = straight;
mawk2311 4:a8dce9e269e5 36
ericoneill 0:a7a8c6ef6d11 37 int main() {
mawk2311 4:a8dce9e269e5 38 //servo.period(SERVO_FREQ);
ericoneill 1:c6fa316ce7d1 39 int integrationCounter = 0;
mawk2311 3:4ba3d22e50dc 40
ericoneill 0:a7a8c6ef6d11 41 while(1) {
mawk2311 3:4ba3d22e50dc 42
mawk2311 4:a8dce9e269e5 43 if(integrationCounter % 151== 0){
ericoneill 1:c6fa316ce7d1 44 si = 1;
ericoneill 2:f3eafd4d3705 45 clk = 1;
ericoneill 2:f3eafd4d3705 46 //wait(.00001);
ericoneill 1:c6fa316ce7d1 47 si = 0;
ericoneill 2:f3eafd4d3705 48 clk = 0;
mawk2311 3:4ba3d22e50dc 49 integrationCounter = 0;
mawk2311 3:4ba3d22e50dc 50
mawk2311 3:4ba3d22e50dc 51 slopeAccum = 0;
mawk2311 3:4ba3d22e50dc 52 slopeCount = 0;
mawk2311 3:4ba3d22e50dc 53 approxPos = 0;
mawk2311 3:4ba3d22e50dc 54 peak1 = 0;
mawk2311 3:4ba3d22e50dc 55 peak2 = 0;
mawk2311 4:a8dce9e269e5 56 risEdge = 0;
mawk2311 4:a8dce9e269e5 57 peakLoc1 = 0;
mawk2311 4:a8dce9e269e5 58 peakLoc2 = 0;
mawk2311 5:60560dd127a2 59 peakVal1 = 0;
mawk2311 5:60560dd127a2 60 peakVal2 = 0;
mawk2311 3:4ba3d22e50dc 61
mawk2311 3:4ba3d22e50dc 62 }
mawk2311 3:4ba3d22e50dc 63 else if (integrationCounter > 129){
mawk2311 4:a8dce9e269e5 64 minVal = 0;
mawk2311 4:a8dce9e269e5 65 for (int c = 0; c < 126; c++) {
mawk2311 4:a8dce9e269e5 66 if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && !peak1 && !risEdge && !peak2){
mawk2311 3:4ba3d22e50dc 67 peak1 = 1;
mawk2311 4:a8dce9e269e5 68 peakLoc1 = c;
mawk2311 5:60560dd127a2 69 peakVal1 = ADCdata[c];
mawk2311 3:4ba3d22e50dc 70 minVal = ADCdata[c];
mawk2311 4:a8dce9e269e5 71 } else if (ADCdata[c] < minVal && peak1 && !peak2){
mawk2311 3:4ba3d22e50dc 72 minVal = ADCdata[c];
mawk2311 4:a8dce9e269e5 73 } else if (ADCdata[c+1] > ADCdata[c] && ADCdata[c+2] > ADCdata[c+1] && peak1 && !peak2){
mawk2311 4:a8dce9e269e5 74 risEdge = 1;
mawk2311 4:a8dce9e269e5 75 } else if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && peak1 && risEdge && !peak2){
mawk2311 3:4ba3d22e50dc 76 peak2 = 1;
mawk2311 4:a8dce9e269e5 77 peakLoc2 = c;
mawk2311 5:60560dd127a2 78 peakVal2 = ADCdata[c];
mawk2311 3:4ba3d22e50dc 79 }
mawk2311 3:4ba3d22e50dc 80 }
mawk2311 4:a8dce9e269e5 81
mawk2311 4:a8dce9e269e5 82 for (int c = peakLoc1; c < peakLoc2; c++) {
mawk2311 4:a8dce9e269e5 83 if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.01f && ADCdata[c] > 0.1f){
mawk2311 3:4ba3d22e50dc 84 slopeAccum += c;
mawk2311 3:4ba3d22e50dc 85 slopeCount++;
mawk2311 3:4ba3d22e50dc 86 }
mawk2311 3:4ba3d22e50dc 87 }
mawk2311 4:a8dce9e269e5 88
mawk2311 3:4ba3d22e50dc 89 approxPos = (float)slopeAccum/(float)slopeCount;
mawk2311 3:4ba3d22e50dc 90
mawk2311 4:a8dce9e269e5 91 if (peak1 && peak2){
mawk2311 5:60560dd127a2 92 if(approxPos > 0 && approxPos <= 40){
mawk2311 5:60560dd127a2 93 if (peakVal2 - peakVal1 > 0.65f){
mawk2311 5:60560dd127a2 94 servo.pulsewidth(hardLeft);
mawk2311 5:60560dd127a2 95 } else {
mawk2311 4:a8dce9e269e5 96 servo.pulsewidth(slightLeft);
mawk2311 4:a8dce9e269e5 97 }
mawk2311 5:60560dd127a2 98 } else if (approxPos > 40 && approxPos <= 60){
mawk2311 5:60560dd127a2 99 servo.pulsewidth(straight);
mawk2311 4:a8dce9e269e5 100 } else if (approxPos > 70 && approxPos <= 128){
mawk2311 5:60560dd127a2 101 if (peakVal1 - peakVal2 > 0.8f){
mawk2311 5:60560dd127a2 102 servo.pulsewidth(hardRight);
mawk2311 5:60560dd127a2 103 } else {
mawk2311 4:a8dce9e269e5 104 servo.pulsewidth(slightRight);
mawk2311 4:a8dce9e269e5 105 }
mawk2311 4:a8dce9e269e5 106 }
mawk2311 3:4ba3d22e50dc 107 }
mawk2311 4:a8dce9e269e5 108 integrationCounter = 150;
ericoneill 0:a7a8c6ef6d11 109 }
ericoneill 2:f3eafd4d3705 110 else{
ericoneill 2:f3eafd4d3705 111 clk = 1;
mawk2311 4:a8dce9e269e5 112 wait_us(50);
mawk2311 3:4ba3d22e50dc 113 ADCdata[integrationCounter - 1] = camData;
ericoneill 2:f3eafd4d3705 114 clk = 0;
ericoneill 2:f3eafd4d3705 115 }
ericoneill 2:f3eafd4d3705 116
ericoneill 2:f3eafd4d3705 117 //clk = 0;
ericoneill 1:c6fa316ce7d1 118 integrationCounter++;
ericoneill 1:c6fa316ce7d1 119 //camData.
ericoneill 0:a7a8c6ef6d11 120
ericoneill 0:a7a8c6ef6d11 121 }
ericoneill 0:a7a8c6ef6d11 122 }