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;
}
