Texas State IEEE / TFC_run_track_oneline

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?

UserRevisionLine numberNew 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 }