void calibcam() { //soll eigentlich noch die Kamera calibrieren, geht aber auch ohne //jetzt wird hierdurch nur das Hauptprogramm gestartet, falls if (active == 0) { //es noch nicht gestartet ist int num = 0; //locale Variablen int valbefore = camData[1]; int val; int gesVal = 0; for (num = 5; num < 124; num++) { //nimmt alle Werte des Camerascans von 5 bis 124, die randwerte werden weggelassen da //diese oft nicht genau sind valbefore = camData[num - 1]; //wert der position direkt daneben if (valbefore > camData[num]) { //berechnet den unterschied zwischen den beiden Werten val = valbefore - camData[num]; } else { val = camData[num] - valbefore; } if (val >= gesVal) { gesVal = val; } } checkLineStep = gesVal * 0.8; //um wv der Wert des nächsten Punkts in camData[128] mindestens abweichen muss, //damit das programm nicht abbricht (damit es eine "Linie" erkennt) TERMINAL_PRINTF("\r\n"); TERMINAL_PRINTF("Calibriert!"); forceAllOff(); //schält alle LEDs aus active = 1; } }
int getBlackPos() { //Methode zur berechnung der Stelle an der es am dunkelsten ist, mitte bei 0 int rawPos = getLowestIndex(camData, 0, 128); //findet die position des tiefsten Werts im Array camData calcMotorSpeed(rawPos); //berechnet die Motorengeschwindkeit anhand dieses Werts int val = getLowestIndex(camData, 1, 128); //berechnet den tiefsten Wert im array, nur zu infozwecken im Terminal int value = (rawPos - 64); //setzt die Mitte bei null TERMINAL_PRINTF("Value: %d", val); TERMINAL_PRINTF(" I Index: %d", rawPos); //gibt die Werte im terminal an return value; //gibt den Zielwert zurück }
void calcdirection() { //methode zur Berechnung der Servoauslenkung float ausl = 0; if (linemissing == 0) { int targetloc = getBlackPos(); //findet heraus wo die Linie ist if (targetloc >= 54 && targetloc <= 74) { Sausl = 0; } else { ausl = (float) targetloc / 64.0 * (float) auslmultiplier; //berechnet die auslenkung um der Linie zu folgen Sausl = ausl; //setzt die globale Variable, mit welcher der Servo angesteuert wird } TERMINAL_PRINTF(" I %d", camData[1]); //schreibt die Werte ins Terminal TERMINAL_PRINTF(" I %d", camData[30]); TERMINAL_PRINTF(" I %d", camData[60]); TERMINAL_PRINTF(" I %d", camData[90]); TERMINAL_PRINTF(" I %d", camData[115]); TERMINAL_PRINTF(" Auslenkung: %d\r\n", (int) (ausl * 10000)); } else { Sausl = Sausl * 2; setMotorSpeed(Motorspeed * 0.8); } }
void checkforLine() { //Methode um zu prüfen ob die Linie noch im sichtfeld ist int num = 0; //locale Variablen int valbefore = camData[1]; int val; int gesVal = 0; for (num = 5; num < 124; num++) { //nimmt alle Werte des Camerascans von 5 bis 124, die randwerte werden weggelassen da //diese oft nicht genau sind valbefore = camData[num - 1]; //wert der position direkt daneben if (valbefore > camData[num]) { //berechnet den unterschied zwischen den beiden Werten val = valbefore - camData[num]; } else { val = camData[num] - valbefore; } if (val >= gesVal) { gesVal = val; } } if (gesVal <= checkLineStep) { //falls der unterschied der beiden Werte zu klein ist wird keine TERMINAL_PRINTF(" LINE MISSING!"); if (linemissing == 0) { //linie gefunden, der automatische Modus wird deaktiviert linemissing = 1; if (TFC_Ticker[3] >= linetimeout) { TFC_Ticker[3] = 0; } } } else { //linie vorhanden, variablen entsprechend setzen active = 1; linemissing = 0; TFC_Ticker[3] = 0; } TERMINAL_PRINTF(" Linestep: %d", gesVal); TERMINAL_PRINTF(" / %d\r\n", checkLineStep); }
int main(void) { uint32_t t,i=0, ana=0; int guardar,dato=0; //int32_t guardar; TFC_Init(); for(;;) { //TFC_Task must be called in your main loop. This keeps certain processing happy (I.E. Serial port queue check) TFC_Task(); //This Demo program will look at the middle 2 switch to select one of 4 demo modes. //Let's look at the middle 2 switches switch(dato) { default: case 0 : TFC_SetMotorPWM(0,0); TFC_SetServo(0,0); //Demo mode 0 just tests the switches and LED's if(TFC_PUSH_BUTTON_1_PRESSED) dato=3; break; case 1: //Demo mode 1 will just move the servos with the on-board potentiometers if(TFC_Ticker[0]>=20) { TFC_Ticker[0] = 0; //reset the Ticker //Every 20 mSeconds, update the Servos TFC_SetServo(0,TFC_ReadPot(0)); TFC_SetServo(1,TFC_ReadPot(1)); } //Let's put a pattern on the LEDs if(TFC_Ticker[1] >= 125) { TFC_Ticker[1] = 0; t++; if(t>4) { t=0; } TFC_SetBatteryLED_Level(t); } TFC_SetMotorPWM(0,0); //Make sure motors are off TFC_HBRIDGE_DISABLE; break; case 2 : //Demo Mode 2 will use the Pots to make the motors move TFC_HBRIDGE_ENABLE; TFC_SetMotorPWM(TFC_ReadPot(0),TFC_ReadPot(0)); //Let's put a pattern on the LEDs if(TFC_Ticker[1] >= 125) { TFC_Ticker[1] = 0; t++; if(t>4) { t=0; } TFC_SetBatteryLED_Level(t); } break; case 3 : if(TFC_PUSH_BUTTON_0_PRESSED){ dato=0; } ana = 4096; if(TFC_Ticker[0]>100 && LineScanImageReady==1) { TFC_Ticker[0] = 0; LineScanImageReady=0; //TERMINAL_PRINTF("\r\n"); //TERMINAL_PRINTF("L:"); if(t==0) t=3; else t--; TFC_SetBatteryLED_Level(t); for(i=0;i<128;i++){ //TERMINAL_PRINTF("%x,",LineScanImage0[i]); //x hexa, i integer, f float...... if(LineScanImage0[i]<ana){ ana = LineScanImage0[i]; guardar = i; } } guardar = guardar - 64; TERMINAL_PRINTF("\r\n"); //TERMINAL_PRINTF("%i",guardar); TFC_SetServo(0,(float)guardar/64.0f); //Rescale to -1.0 to 1.0 } TFC_HBRIDGE_ENABLE; if((float)guardar < -5 && (float)guardar > 5){ if(guardar < 0){ guardar = guardar*(-1); } TFC_SetMotorPWM((float)guardar/100.0f,(float)guardar/100.0f); } break; } } return 0; }