![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
xx
Fork of PES1 by
Roboter.cpp
- Committer:
- itslinear
- Date:
- 2017-05-09
- Revision:
- 18:6547e54ac803
- Parent:
- 17:caf5ae550f2e
File content as of revision 18:6547e54ac803:
#include "Roboter.h" Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver) { sensors[0].init( distance, bit0, bit1, bit2, 0); sensors[1].init( distance, bit0, bit1, bit2, 1); sensors[2].init( distance, bit0, bit1, bit2, 2); sensors[3].init( distance, bit0, bit1, bit2, 3); sensors[4].init( distance, bit0, bit1, bit2, 4); sensors[5].init( distance, bit0, bit1, bit2, 5); this->pwmR = pwmR; this->pwmL = pwmL; this->servo1 = servo1; this->servo2 = servo2; this->counterLeft = counterLeft; this->counterRight = counterRight; this->enableMotorDriver = enableMotorDriver; initSpeedcontrol(); // t1.attach(this, &Roboter::speedCtrl, PERIOD); } float Roboter::readSensor1() // Sensoren auslesen { return sensors[0].read(); } void Roboter::bandeAusweichen() // Hindernissen ausweichen { float x=0.13f; // Distanz ab welcher sensoren reagieren sollen while(sensors[0] < x && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten desiredSpeedLeft = -v; desiredSpeedRight = v; } while(sensors[0] < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links desiredSpeedLeft = -v; desiredSpeedRight = -v; } while(sensors[1] < x) { // sensor rechts, roboter dreht nach links desiredSpeedLeft = -v; desiredSpeedRight = -v; } while(sensors[5] < x && sensors[1]>(x+0.02f)) { // sensor links, roboter dreht nach rechts desiredSpeedLeft = v; desiredSpeedRight = v; } } int Roboter::pickup() // Klotz aufnehmen { static int i = 0; int r; // Rückegabewert desiredSpeedLeft = 0; desiredSpeedRight = 0; // while(i < 260) { if(i < 70) { // Arm nach unten fahren servo2->write(0.96f); } if(i > 69 && i < 90) { // gerade fahren desiredSpeedLeft = v; desiredSpeedRight = -v; } if(i > 89 && i < 140) { // gerade fahren + Greifer schliessen desiredSpeedLeft = v; desiredSpeedRight = -v; servo1->write(0.0f); } if(i > 139 && i < 210) { // Arm nach oben fahren desiredSpeedLeft = 0; desiredSpeedRight = 0; servo2->write(0.0f); } if(i > 209 && i < 260) { // Greifer öffnen servo1->write(0.5f); } i++; //wait(0.01f); // } r = 5; // Rückegabewert if( i > 260 ) r = 0; return r; } void Roboter::anfahren(float cam) { float speedValue; float maxWert = 20; if(cam == 100) { desiredSpeedLeft = -v/2; desiredSpeedRight = -v/2; } if(cam == 200) { desiredSpeedLeft = v/2; desiredSpeedRight = v/2; } if(cam > 100 && cam <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben cam = cam -99.0f; // cam nimmt die Werte von 1 bis 100 an speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's desiredSpeedLeft = -speedValue; desiredSpeedRight = -speedValue; } if(cam > 200 && cam < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben cam = cam -199.0f; // cam nimmt die Werte von 1 bis 100 an speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's desiredSpeedLeft = speedValue; desiredSpeedRight = speedValue; } if(cam >= 300 && cam <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben cam = cam-299.0f; // cam nimmt die Werte von 1 bis 100 an speedValue = v*1.5f + (cam * (maxWert /100.0f)); // Berechnung des speedValue's desiredSpeedLeft = speedValue; desiredSpeedRight = -speedValue; } } int Roboter::geradeFahren() { int r; static int time2 = 0; if(time2 < 200) { // 1s gerade aus fahren desiredSpeedLeft = v*3; desiredSpeedRight = -v*3; time2 ++; r = 1; // state wait(0.01f); } else { time2 = 0; desiredSpeedLeft = 0.0f; desiredSpeedRight = 0.0f; r = 2; //state } return r; } int Roboter::kreisFahren() { int r; static int time1 = 0; if(time1 < 310) { // 1s im Kreis drehen desiredSpeedLeft = v*1.7f; desiredSpeedRight = v*1.7f; time1 ++; r = 1; // state wait(0.01f); } else { time1 = 0; desiredSpeedLeft = 0.0f; desiredSpeedRight = 0.0f; r = 3; // state } return r; } int Roboter::stateAuswahl(float cam, int temp) { int s; if(cam >= 100.0f && cam < 400.0f ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399 s = 4; } if(cam == 400.0f) { // Roboter in Position s = 5; } if(cam == 0.0f) { s = temp; } return s; /*int s; if(cam == 1 || cam == 2 || cam == 3) { // Die Kamera erkennt ein grünen Klotz s = 4; } if(cam == 4) { // Roboter in Position s = 5; } if(cam == 0) { // kein Klotz in Sicht s = temp; } return s;*/ } void Roboter::initSpeedcontrol() { // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us pwmL->period(0.00005f); // Setzt die Periode auf 50 μs pwmR->period(0.00005f); *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us *pwmR = 0.5f; // Duty-Cycle von 50% // Initialisieren von lokalen Variabeln previousValueCounterLeft = counterLeft->read(); previousValueCounterRight = counterRight->read(); speedLeftFilter.setPeriod(PERIOD); speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); speedRightFilter.setPeriod(PERIOD); speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); desiredSpeedLeft = 0.0f; desiredSpeedRight = 0.0f; actualSpeedLeft = 0.0f; actualSpeedRight = 0.0f; *enableMotorDriver = 1; } void Roboter::speedCtrl() { // Berechnen die effektiven Drehzahlen der Motoren in [rpm] short valueCounterLeft = counterLeft->read(); short valueCounterRight = counterRight->read(); short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft; short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight; previousValueCounterLeft = valueCounterLeft; previousValueCounterRight = valueCounterRight; actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f); actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f); // Berechnen der Motorspannungen Uout float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN; float voltageRight = KP*(desiredSpeedRight*correction-actualSpeedRight)+desiredSpeedRight*correction/KN; // Berechnen, Limitieren und Setzen der Duty-Cycle float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; *pwmL = dutyCycleLeft; float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; *pwmR = dutyCycleRight; }