For use of the TFC teams
Dependencies: FRDM-TFC
TFC_run_track_oneline.cpp@0:aebd709fc6bb, 2015-01-30 (annotated)
- Committer:
- jmar11
- Date:
- Fri Jan 30 18:16:40 2015 +0000
- Revision:
- 0:aebd709fc6bb
First
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jmar11 | 0:aebd709fc6bb | 1 | #include "mbed.h" |
jmar11 | 0:aebd709fc6bb | 2 | #include "TFC.h" |
jmar11 | 0:aebd709fc6bb | 3 | |
jmar11 | 0:aebd709fc6bb | 4 | //Serial pc(USBTX, USBRX); |
jmar11 | 0:aebd709fc6bb | 5 | |
jmar11 | 0:aebd709fc6bb | 6 | float servoTrim(); |
jmar11 | 0:aebd709fc6bb | 7 | float motorTrim(); |
jmar11 | 0:aebd709fc6bb | 8 | float avgPos(); |
jmar11 | 0:aebd709fc6bb | 9 | int readCamCenter(); |
jmar11 | 0:aebd709fc6bb | 10 | |
jmar11 | 0:aebd709fc6bb | 11 | int cam[122], |
jmar11 | 0:aebd709fc6bb | 12 | peaks[2]; |
jmar11 | 0:aebd709fc6bb | 13 | |
jmar11 | 0:aebd709fc6bb | 14 | int filter[]={-1,-1,-1,6,-1,-1,-1}; //size must not change |
jmar11 | 0:aebd709fc6bb | 15 | |
jmar11 | 0:aebd709fc6bb | 16 | float servBuf[100] = {0}, |
jmar11 | 0:aebd709fc6bb | 17 | weight[100]; |
jmar11 | 0:aebd709fc6bb | 18 | |
jmar11 | 0:aebd709fc6bb | 19 | int main(){ |
jmar11 | 0:aebd709fc6bb | 20 | float pos = 0, |
jmar11 | 0:aebd709fc6bb | 21 | speed = 0; |
jmar11 | 0:aebd709fc6bb | 22 | |
jmar11 | 0:aebd709fc6bb | 23 | int prevState1 = 0, |
jmar11 | 0:aebd709fc6bb | 24 | prevState2 = 0; |
jmar11 | 0:aebd709fc6bb | 25 | |
jmar11 | 0:aebd709fc6bb | 26 | |
jmar11 | 0:aebd709fc6bb | 27 | weight[0] = 1; |
jmar11 | 0:aebd709fc6bb | 28 | for(int i = 1; i < 100; i++){ |
jmar11 | 0:aebd709fc6bb | 29 | weight[i] = weight[i-1] * 0.95; |
jmar11 | 0:aebd709fc6bb | 30 | } |
jmar11 | 0:aebd709fc6bb | 31 | |
jmar11 | 0:aebd709fc6bb | 32 | //pc.baud(115200); |
jmar11 | 0:aebd709fc6bb | 33 | TFC_Init(); |
jmar11 | 0:aebd709fc6bb | 34 | |
jmar11 | 0:aebd709fc6bb | 35 | while(TFC_ReadPushButton(0) == 0){ |
jmar11 | 0:aebd709fc6bb | 36 | } |
jmar11 | 0:aebd709fc6bb | 37 | |
jmar11 | 0:aebd709fc6bb | 38 | wait(3); |
jmar11 | 0:aebd709fc6bb | 39 | |
jmar11 | 0:aebd709fc6bb | 40 | TFC_HBRIDGE_ENABLE; |
jmar11 | 0:aebd709fc6bb | 41 | |
jmar11 | 0:aebd709fc6bb | 42 | while(1){ |
jmar11 | 0:aebd709fc6bb | 43 | if(TFC_ReadPushButton(0) != 0 && prevState1 != TFC_ReadPushButton(0)){ |
jmar11 | 0:aebd709fc6bb | 44 | speed -= 0.05; |
jmar11 | 0:aebd709fc6bb | 45 | } |
jmar11 | 0:aebd709fc6bb | 46 | else if(TFC_ReadPushButton(1) != 0 && prevState2 != TFC_ReadPushButton(1)){ |
jmar11 | 0:aebd709fc6bb | 47 | speed += 0.05; |
jmar11 | 0:aebd709fc6bb | 48 | } |
jmar11 | 0:aebd709fc6bb | 49 | |
jmar11 | 0:aebd709fc6bb | 50 | prevState1 = TFC_ReadPushButton(0); |
jmar11 | 0:aebd709fc6bb | 51 | prevState2 = TFC_ReadPushButton(1); |
jmar11 | 0:aebd709fc6bb | 52 | |
jmar11 | 0:aebd709fc6bb | 53 | TFC_SetMotorPWM(speed, speed); |
jmar11 | 0:aebd709fc6bb | 54 | |
jmar11 | 0:aebd709fc6bb | 55 | pos = float(readCamCenter() - 61) / 61; |
jmar11 | 0:aebd709fc6bb | 56 | |
jmar11 | 0:aebd709fc6bb | 57 | pos *= (TFC_ReadPot(1) + 1) / 2; |
jmar11 | 0:aebd709fc6bb | 58 | |
jmar11 | 0:aebd709fc6bb | 59 | pos += TFC_ReadPot(0); |
jmar11 | 0:aebd709fc6bb | 60 | |
jmar11 | 0:aebd709fc6bb | 61 | if((TFC_ServoTicker % 5) == 0){ |
jmar11 | 0:aebd709fc6bb | 62 | for(int i = 99; i > 0; i--){ |
jmar11 | 0:aebd709fc6bb | 63 | servBuf[i] = servBuf[i-1]; |
jmar11 | 0:aebd709fc6bb | 64 | } |
jmar11 | 0:aebd709fc6bb | 65 | servBuf[0] = pos; |
jmar11 | 0:aebd709fc6bb | 66 | } |
jmar11 | 0:aebd709fc6bb | 67 | |
jmar11 | 0:aebd709fc6bb | 68 | TFC_SetServo(0, avgPos()); |
jmar11 | 0:aebd709fc6bb | 69 | } |
jmar11 | 0:aebd709fc6bb | 70 | } |
jmar11 | 0:aebd709fc6bb | 71 | |
jmar11 | 0:aebd709fc6bb | 72 | float servoTrim(){ |
jmar11 | 0:aebd709fc6bb | 73 | return TFC_ReadPot(0); |
jmar11 | 0:aebd709fc6bb | 74 | } |
jmar11 | 0:aebd709fc6bb | 75 | |
jmar11 | 0:aebd709fc6bb | 76 | float motorTrim(){ |
jmar11 | 0:aebd709fc6bb | 77 | return TFC_ReadPot(1); |
jmar11 | 0:aebd709fc6bb | 78 | } |
jmar11 | 0:aebd709fc6bb | 79 | |
jmar11 | 0:aebd709fc6bb | 80 | int readCamCenter(){ |
jmar11 | 0:aebd709fc6bb | 81 | int temp; |
jmar11 | 0:aebd709fc6bb | 82 | int peaksval[] ={0, 0}; |
jmar11 | 0:aebd709fc6bb | 83 | |
jmar11 | 0:aebd709fc6bb | 84 | if(TFC_LineScanImageReady != 0){ |
jmar11 | 0:aebd709fc6bb | 85 | |
jmar11 | 0:aebd709fc6bb | 86 | TFC_LineScanImageReady = 0; |
jmar11 | 0:aebd709fc6bb | 87 | for(int i = 0; i < 122; i++){ |
jmar11 | 0:aebd709fc6bb | 88 | temp = 0; |
jmar11 | 0:aebd709fc6bb | 89 | for(int h = 0; h < 7; h++){ |
jmar11 | 0:aebd709fc6bb | 90 | temp = temp + (TFC_LineScanImage0[i+h] * filter[h]); |
jmar11 | 0:aebd709fc6bb | 91 | } |
jmar11 | 0:aebd709fc6bb | 92 | cam[i] = temp; |
jmar11 | 0:aebd709fc6bb | 93 | } |
jmar11 | 0:aebd709fc6bb | 94 | } |
jmar11 | 0:aebd709fc6bb | 95 | |
jmar11 | 0:aebd709fc6bb | 96 | temp = 0; |
jmar11 | 0:aebd709fc6bb | 97 | |
jmar11 | 0:aebd709fc6bb | 98 | for(int i = 0; i < 2; i++){ |
jmar11 | 0:aebd709fc6bb | 99 | peaksval[i] = 0; |
jmar11 | 0:aebd709fc6bb | 100 | } |
jmar11 | 0:aebd709fc6bb | 101 | |
jmar11 | 0:aebd709fc6bb | 102 | for(int i = 0; i < 122; i++){ |
jmar11 | 0:aebd709fc6bb | 103 | if(cam[i] > peaksval[0]){ |
jmar11 | 0:aebd709fc6bb | 104 | peaksval[0] = cam[i]; |
jmar11 | 0:aebd709fc6bb | 105 | peaks[0] = i; |
jmar11 | 0:aebd709fc6bb | 106 | } |
jmar11 | 0:aebd709fc6bb | 107 | } |
jmar11 | 0:aebd709fc6bb | 108 | |
jmar11 | 0:aebd709fc6bb | 109 | for(int i = 0; i < 122; i++){ |
jmar11 | 0:aebd709fc6bb | 110 | if(cam[i] > peaksval[1] && peaks[0] != i){ |
jmar11 | 0:aebd709fc6bb | 111 | peaksval[1] = cam[i]; |
jmar11 | 0:aebd709fc6bb | 112 | peaks[1] = i; |
jmar11 | 0:aebd709fc6bb | 113 | } |
jmar11 | 0:aebd709fc6bb | 114 | } |
jmar11 | 0:aebd709fc6bb | 115 | |
jmar11 | 0:aebd709fc6bb | 116 | temp = ((peaks[0]+1)+(peaks[1]+1))/2; |
jmar11 | 0:aebd709fc6bb | 117 | |
jmar11 | 0:aebd709fc6bb | 118 | return temp; |
jmar11 | 0:aebd709fc6bb | 119 | } |
jmar11 | 0:aebd709fc6bb | 120 | |
jmar11 | 0:aebd709fc6bb | 121 | float avgPos(){ |
jmar11 | 0:aebd709fc6bb | 122 | float temp = 0; |
jmar11 | 0:aebd709fc6bb | 123 | |
jmar11 | 0:aebd709fc6bb | 124 | for(int i = 0; i < 100; i++){ |
jmar11 | 0:aebd709fc6bb | 125 | temp += servBuf[i] * weight[i]; |
jmar11 | 0:aebd709fc6bb | 126 | } |
jmar11 | 0:aebd709fc6bb | 127 | |
jmar11 | 0:aebd709fc6bb | 128 | return (temp / 19.8816); |
jmar11 | 0:aebd709fc6bb | 129 | } |