![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
für holdor
Fork of PES by
main.cpp
- Committer:
- schneju2
- Date:
- 2017-05-01
- Revision:
- 20:c961dc550882
- Parent:
- 17:caf5ae550f2e
- Child:
- 22:ffd36f07c046
File content as of revision 20:c961dc550882:
#include <mbed.h> #include "Roboter.h" DigitalOut led(LED1); //Periphery for distance sensors AnalogIn distance(PB_1); DigitalOut enable(PC_1); DigitalOut bit0(PH_1); DigitalOut bit1(PC_2); DigitalOut bit2(PC_3); IRSensor sensors[6]; // Periphery for servos Servo servos1(PB_7); Servo servos2(PA_6); //Periphery for Switch ON/OFF DigitalIn switchOnOff(USER_BUTTON); //motor stuff DigitalOut enableMotorDriver(PB_2); PwmOut pwmL( PA_8 ); PwmOut pwmR( PA_9 ); //indicator leds arround robot DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servos1,&servos2); int main() { enable = 1; // Sensoren einschalten enableMotorDriver = 1; // Schaltet die Motoren ein pwmL.period(0.00005f); // Setzt die Periode auf 50 μs pwmR.period(0.00005f); // Setzt die Periode auf 50 μs pwmL = 0.5f; // Geschwindikeit auf 0 pwmR = 0.5f; // Geschwindikeit auf 0 int state = 0; // Diese Variable gibt an in welchem State man sich befindet int tempState = 2; // int time1 = 0; // Die Zeit wird auf 0 gesetzt int time2 = 0; // Die Zeit wird auf 0 gesetzt while(1) { double camValue = readCamera(); switch(state) { case 0: // Roboter Anschalten mit Knopf printf("0"); if (switchOnOff == 0) { state = 1; } break; case 1: printf("1"); if(camValue >= 100 && camValue < 400 ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399 state = 4; } if(camValue == 400) { // Roboter in Position state = 5; } if(camValue == 0){ state = tempState; } break; case 2: printf("2"); if(time1 < 20) { // Im Kreis drehen für 1s pwmL = 0.4f; pwmR = 0.4f; time1 ++; state = 1; tempState = 2; } else { time1 = 0; pwmL = 0.5f; pwmR = 0.5f; state = 3; } break; case 3: printf("3"); roboter1.bandeAusweichen(); if(time2 < 40) { // gerade aus fahren pwmL = 0.6f; pwmR = 0.4f; time2 ++; state = 1; tempState = 3; } else { time2 = 0; pwmL = 0.5f; pwmR = 0.5f; state = 2; } break; case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren printf("4"); double maxWert = 0.3; // Maximale Werte für die pmL und pmR roboter1.bandeAusweichen(); if(camValue >= 100 && camValue <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben camValue = camValue -99; double speedValue = camValue * (maxWert /100.0); pwmL = 0.45f; pwmR = 0.45f; } if(camValue >=200 && camValue < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben camValue = camValue -199; double speedValue = camValue * (maxWert /100.0); pwmL = 0.5f + speedValue; pwmR = 0.5f + speedValue; } if(camValue >= 300 && camValue <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben camValue = camValue-299; // CamValue nimmt die Werte von 1 bis 100 an double speedValue = camValue * (maxWert /100.0); // Berechnung des speedValue's pwmL = 0.5f + speedValue; pwmR = 0.5f - speedValue; } state = 1; break; case 5: // Aufnehmen des Klotzes printf("5"); pwmL = 0.5f; pwmR = 0.5f; state = 1; time1 = 0; time2 = 0; break; default: break; } //wait(0.1f); } }