E=MC / Mbed 2 deprecated linecam_practice

Dependencies:   mbed

Committer:
mawk2311
Date:
Fri Mar 20 04:39:52 2015 +0000
Revision:
6:53d521496469
Parent:
5:60560dd127a2
Child:
7:f7f4308c7ac0
Modified the line detect.

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