For use of the TFC teams
Dependencies: FRDM-TFC
TFC_run_track_oneline.cpp
- Committer:
- jmar11
- Date:
- 2015-01-30
- Revision:
- 0:aebd709fc6bb
File content as of revision 0:aebd709fc6bb:
#include "mbed.h" #include "TFC.h" //Serial pc(USBTX, USBRX); float servoTrim(); float motorTrim(); float avgPos(); int readCamCenter(); int cam[122], peaks[2]; int filter[]={-1,-1,-1,6,-1,-1,-1}; //size must not change float servBuf[100] = {0}, weight[100]; int main(){ float pos = 0, speed = 0; int prevState1 = 0, prevState2 = 0; weight[0] = 1; for(int i = 1; i < 100; i++){ weight[i] = weight[i-1] * 0.95; } //pc.baud(115200); TFC_Init(); while(TFC_ReadPushButton(0) == 0){ } wait(3); TFC_HBRIDGE_ENABLE; while(1){ if(TFC_ReadPushButton(0) != 0 && prevState1 != TFC_ReadPushButton(0)){ speed -= 0.05; } else if(TFC_ReadPushButton(1) != 0 && prevState2 != TFC_ReadPushButton(1)){ speed += 0.05; } prevState1 = TFC_ReadPushButton(0); prevState2 = TFC_ReadPushButton(1); TFC_SetMotorPWM(speed, speed); pos = float(readCamCenter() - 61) / 61; pos *= (TFC_ReadPot(1) + 1) / 2; pos += TFC_ReadPot(0); if((TFC_ServoTicker % 5) == 0){ for(int i = 99; i > 0; i--){ servBuf[i] = servBuf[i-1]; } servBuf[0] = pos; } TFC_SetServo(0, avgPos()); } } float servoTrim(){ return TFC_ReadPot(0); } float motorTrim(){ return TFC_ReadPot(1); } int readCamCenter(){ int temp; int peaksval[] ={0, 0}; if(TFC_LineScanImageReady != 0){ TFC_LineScanImageReady = 0; for(int i = 0; i < 122; i++){ temp = 0; for(int h = 0; h < 7; h++){ temp = temp + (TFC_LineScanImage0[i+h] * filter[h]); } cam[i] = temp; } } temp = 0; for(int i = 0; i < 2; i++){ peaksval[i] = 0; } for(int i = 0; i < 122; i++){ if(cam[i] > peaksval[0]){ peaksval[0] = cam[i]; peaks[0] = i; } } for(int i = 0; i < 122; i++){ if(cam[i] > peaksval[1] && peaks[0] != i){ peaksval[1] = cam[i]; peaks[1] = i; } } temp = ((peaks[0]+1)+(peaks[1]+1))/2; return temp; } float avgPos(){ float temp = 0; for(int i = 0; i < 100; i++){ temp += servBuf[i] * weight[i]; } return (temp / 19.8816); }