![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
hallo
Fork of PES1 by
Roboter.cpp
- Committer:
- itslinear
- Date:
- 2017-05-13
- Revision:
- 20:a90e5b54bf7b
- Parent:
- 19:adae367247d4
- Child:
- 21:69ee872b8ee9
File content as of revision 20:a90e5b54bf7b:
#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); } int Roboter::readSensors() // Sensoren auslesen { int lr = 1; float x = 0.13; // Distanz ab welcher sensoren reagieren sollen if(sensors[1] < x || sensors[5] < x) { // Sensor rechts und links lr = 6; } //printf("%d",lr); return lr; } void Roboter::ausweichen1() // Hindernissen ausweichen { float x = 0.15; // Distanz ab welcher sensoren reagieren sollen while(sensors[0] < (x-0.02f) && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten desiredSpeedLeft = -v*2; desiredSpeedRight = v*2; } while(sensors[0] < (x-0.02f) && sensors[5] > x) { // sensor vorne, roboter dreht nach links desiredSpeedLeft = -v*2; desiredSpeedRight = -v*2; } while(sensors[1] < x) { // sensor rechts, roboter dreht nach links desiredSpeedLeft = -v*2; desiredSpeedRight = -v*2; } while(sensors[5] < x && sensors[1] > x) { // sensor links, roboter dreht nach rechts desiredSpeedLeft = v*2; desiredSpeedRight = v*2; } } int Roboter::ausweichen2() // Hindernissen ausweichen beim Anfahren des Klotzes { float x = 0.13; // Distanz ab welcher sensoren reagieren sollen static int l = 0; static int r = 0; static int ii = 0; int s = 6; if(sensors[1] < x && l == 0) { r = 1; } if(sensors[5] < x && r == 0) { l = 1; } if(r == 1) { if(ii < 500) { // linksdrehend rückwärts fahren desiredSpeedLeft = -(v+30); desiredSpeedRight = v+30; } if(ii > 499 && ii < 1100) { // links drehen desiredSpeedLeft = -v*2; desiredSpeedRight = -v*2; } if(ii > 1099 && ii < 2900) { // gerade fahren desiredSpeedLeft = v*2; desiredSpeedRight = -v*2; } if(ii > 2899 && ii < 3900) { // rechts drehen desiredSpeedLeft = v*2; desiredSpeedRight = v*2; } ii++; if(ii > 3899) { desiredSpeedLeft = 0; desiredSpeedRight = 0; r = 0; ii = 0; s = 1; } } if(l == 1) { if(ii < 500) { // rückwärts fahren desiredSpeedLeft = -(v+30); desiredSpeedRight = v+30; } if(ii > 499 && ii < 1100) { // rechts drehen desiredSpeedLeft = v*2; desiredSpeedRight = v*2; } if(ii > 1099 && ii < 2900) { // gerade fahren desiredSpeedLeft = v*2; desiredSpeedRight = -v*2; } if(ii > 2899 && ii < 3900) { // links drehen desiredSpeedLeft = -v*2; desiredSpeedRight = -v*2; } ii++; if(ii > 3899) { desiredSpeedLeft = 0; desiredSpeedRight = 0; l = 0; ii = 0; s = 1; } } return s; } int Roboter::pickup() // Klotz aufnehmen { static int ii = 0; int r; // Rückegabewert desiredSpeedLeft = 0; desiredSpeedRight = 0; if(ii < 650) { // Arm nach unten fahren servo2->SetPosition(2250); } if(ii > 649 && ii < 1100) { // gerade fahren desiredSpeedLeft = v; desiredSpeedRight = -v; } if(ii > 1099 && ii < 1500) { // gerade fahren + Greifer schliessen if(ii < 1400) { desiredSpeedLeft = v; desiredSpeedRight = -v; } servo1->SetPosition(500); } if(ii > 1499 && ii < 1900) { // rückwärts fahren desiredSpeedLeft = -v; desiredSpeedRight = v; } if(ii > 1899 && ii < 2550) { // Arm nach oben fahren desiredSpeedLeft = 0; desiredSpeedRight = 0; servo2->SetPosition(700); } if(ii > 2549 && ii < 3000) { // Greifer öffnen servo1->SetPosition(1500); } ii++; r = 5; // Rückegabewert if( ii > 2999 ) { r = 1; ii = 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*3) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's desiredSpeedLeft = speedValue; desiredSpeedRight = -speedValue; } } int Roboter::geradeFahren() { int r; static int time2 = 0; if(time2 < 10) { // 1s gerade aus fahren desiredSpeedLeft = v*4; desiredSpeedRight = -v*4; time2 ++; r = 1; // state } else { time2 = 0; desiredSpeedLeft = 0.0f; desiredSpeedRight = 0.0f; r = 2; //state } return r; } int Roboter::back() { static int ii = 0; int s = 7; if(ii < 1000) { desiredSpeedLeft = -v; desiredSpeedRight = v; } if(ii > 999 && ii < 2000) { desiredSpeedLeft = v*2; desiredSpeedRight = v*2; } ii++; if(ii > 1999) { desiredSpeedLeft = 0; desiredSpeedRight = 0; ii = 0; s = 1; } return s; } int Roboter::kreisFahren() { int r; static int time1 = 0; if(time1 < 10) { // 1s im Kreis drehen desiredSpeedLeft = v*1.7f; desiredSpeedRight = v*1.7f; time1 ++; r = 1; // state } else { time1 = 0; desiredSpeedLeft = 0.0f; desiredSpeedRight = 0.0f; r = 3; // state } return r; } int Roboter::stateAuswahl(float cam, int tempState, int temp) { int s = 1; 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 && temp != 5) { // Roboter in Position s = 5; } if(cam == 400.0f && temp == 5) { s = 7; } if(cam < 1) { s = tempState; } if(cam == 1) { s = 1; } 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; }