![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
xx
Fork of PES1 by
Revision 18:6547e54ac803, committed 2017-05-09
- Comitter:
- itslinear
- Date:
- Tue May 09 13:21:49 2017 +0000
- Parent:
- 17:caf5ae550f2e
- Commit message:
- f?r holdi;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EncoderCounter/EncoderCounter.cpp Tue May 09 13:21:49 2017 +0000 @@ -0,0 +1,139 @@ +/* + * EncoderCounter.cpp + * Copyright (c) 2016, ZHAW + * All rights reserved. + */ + +#include "EncoderCounter.h" + +using namespace std; + +/** + * Creates and initializes the driver to read the quadrature + * encoder counter of the STM32 microcontroller. + * @param a the input pin for the channel A. + * @param b the input pin for the channel B. + */ +EncoderCounter::EncoderCounter(PinName a, PinName b) { + + // check pins + + if ((a == PA_6) && (b == PC_7)) { + + // pinmap OK for TIM3 CH1 and CH2 + + TIM = TIM3; + + // configure reset and clock control registers + + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // manually enable port C (port A enabled by mbed library) + + // configure general purpose I/O registers + + GPIOA->MODER &= ~GPIO_MODER_MODER6; // reset port A6 + GPIOA->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port A6 + GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port A6 + GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down + GPIOA->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port A6 + GPIOA->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port A6 + + GPIOC->MODER &= ~GPIO_MODER_MODER7; // reset port C7 + GPIOC->MODER |= GPIO_MODER_MODER7_1; // set alternate mode of port C7 + GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7; // reset pull-up/pull-down on port C7 + GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1; // set input as pull-down + GPIOC->AFR[0] &= ~0xF0000000; // reset alternate function of port C7 + GPIOC->AFR[0] |= 2 << 4*7; // set alternate funtion 2 of port C7 + + // configure reset and clock control registers + + RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST; //reset TIM3 controller + RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST; + + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // TIM3 clock enable + + } else if ((a == PB_6) && (b == PB_7)) { + + // pinmap OK for TIM4 CH1 and CH2 + + TIM = TIM4; + + // configure reset and clock control registers + + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // manually enable port B (port A enabled by mbed library) + + // configure general purpose I/O registers + + GPIOB->MODER &= ~GPIO_MODER_MODER6; // reset port B6 + GPIOB->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port B6 + GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port B6 + GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down + GPIOB->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port B6 + GPIOB->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port B6 + + GPIOB->MODER &= ~GPIO_MODER_MODER7; // reset port B7 + GPIOB->MODER |= GPIO_MODER_MODER7_1; // set alternate mode of port B7 + GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7; // reset pull-up/pull-down on port B7 + GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1; // set input as pull-down + GPIOB->AFR[0] &= ~0xF0000000; // reset alternate function of port B7 + GPIOB->AFR[0] |= 2 << 4*7; // set alternate funtion 2 of port B7 + + // configure reset and clock control registers + + RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST; //reset TIM4 controller + RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST; + + RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // TIM4 clock enable + + } else { + + printf("pinmap not found for peripheral\n"); + } + + // configure general purpose timer 3 or 4 + + TIM->CR1 = 0x0000; // counter disable + TIM->CR2 = 0x0000; // reset master mode selection + TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges + TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0; + TIM->CCMR2 = 0x0000; // reset capture mode register 2 + TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E; + TIM->CNT = 0x0000; // reset counter value + TIM->ARR = 0xFFFF; // auto reload register + TIM->CR1 = TIM_CR1_CEN; // counter enable +} + +EncoderCounter::~EncoderCounter() {} + +/** + * Resets the counter value to zero. + */ +void EncoderCounter::reset() { + + TIM->CNT = 0x0000; +} + +/** + * Resets the counter value to a given offset value. + * @param offset the offset value to reset the counter to. + */ +void EncoderCounter::reset(short offset) { + + TIM->CNT = -offset; +} + +/** + * Reads the quadrature encoder counter value. + * @return the quadrature encoder counter as a signed 16-bit integer value. + */ +short EncoderCounter::read() { + + return (short)(-TIM->CNT); +} + +/** + * The empty operator is a shorthand notation of the <code>read()</code> method. + */ +EncoderCounter::operator short() { + + return read(); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EncoderCounter/EncoderCounter.h Tue May 09 13:21:49 2017 +0000 @@ -0,0 +1,33 @@ +/* + * EncoderCounter.h + * Copyright (c) 2016, ZHAW + * All rights reserved. + */ + +#ifndef ENCODER_COUNTER_H_ +#define ENCODER_COUNTER_H_ + +#include <cstdlib> +#include <mbed.h> + +/** + * This class implements a driver to read the quadrature + * encoder counter of the STM32 microcontroller. + */ +class EncoderCounter { + + public: + + EncoderCounter(PinName a, PinName b); + virtual ~EncoderCounter(); + void reset(); + void reset(short offset); + short read(); + operator short(); + + private: + + TIM_TypeDef* TIM; +}; + +#endif /* ENCODER_COUNTER_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EncoderCounter/LowpassFilter.cpp Tue May 09 13:21:49 2017 +0000 @@ -0,0 +1,110 @@ +/* + * LowpassFilter.cpp + * Copyright (c) 2016, ZHAW + * All rights reserved. + */ + +#include "LowpassFilter.h" + +using namespace std; + +/** + * Creates a LowpassFilter object with a default corner frequency of 1000 [rad/s]. + */ +LowpassFilter::LowpassFilter() { + + period = 1.0f; + frequency = 1000.0f; + + a11 = (1.0f+frequency*period)*exp(-frequency*period); + a12 = period*exp(-frequency*period); + a21 = -frequency*frequency*period*exp(-frequency*period); + a22 = (1.0f-frequency*period)*exp(-frequency*period); + b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; + b2 = period*exp(-frequency*period); + + x1 = 0.0f; + x2 = 0.0f; +} + +/** + * Deletes the LowpassFilter object. + */ +LowpassFilter::~LowpassFilter() {} + +/** + * Resets the filtered value to zero. + */ +void LowpassFilter::reset() { + + x1 = 0.0f; + x2 = 0.0f; +} + +/** + * Resets the filtered value to a given value. + * @param value the value to reset the filter to. + */ +void LowpassFilter::reset(float value) { + + x1 = value/frequency/frequency; + x2 = (x1-a11*x1-b1*value)/a12; +} + +/** + * Sets the sampling period of the filter. + * This is typically the sampling period of the realtime thread of a controller that uses this filter. + * @param the sampling period, given in [s]. + */ +void LowpassFilter::setPeriod(float period) { + + this->period = period; + + a11 = (1.0f+frequency*period)*exp(-frequency*period); + a12 = period*exp(-frequency*period); + a21 = -frequency*frequency*period*exp(-frequency*period); + a22 = (1.0f-frequency*period)*exp(-frequency*period); + b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; + b2 = period*exp(-frequency*period); +} + +/** + * Sets the corner frequency of this filter. + * @param frequency the corner frequency of the filter in [rad/s]. + */ +void LowpassFilter::setFrequency(float frequency) { + + this->frequency = frequency; + + a11 = (1.0f+frequency*period)*exp(-frequency*period); + a12 = period*exp(-frequency*period); + a21 = -frequency*frequency*period*exp(-frequency*period); + a22 = (1.0f-frequency*period)*exp(-frequency*period); + b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; + b2 = period*exp(-frequency*period); +} + +/** + * Gets the current corner frequency of this filter. + * @return the current corner frequency in [rad/s]. + */ +float LowpassFilter::getFrequency() { + + return frequency; +} + +/** + * Filters a value. + * @param value the original unfiltered value. + * @return the filtered value. + */ +float LowpassFilter::filter(float value) { + + float x1old = x1; + float x2old = x2; + + x1 = a11*x1old+a12*x2old+b1*value; + x2 = a21*x1old+a22*x2old+b2*value; + + return frequency*frequency*x1; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EncoderCounter/LowpassFilter.h Tue May 09 13:21:49 2017 +0000 @@ -0,0 +1,39 @@ +/* + * LowpassFilter.h + * Copyright (c) 2016, ZHAW + * All rights reserved. + */ + +#ifndef LOWPASS_FILTER_H_ +#define LOWPASS_FILTER_H_ + +#include <cstdlib> +#include <cmath> + +/** + * This class implements a time-discrete 2nd order low-pass filter for a series of data values. + * This filter can typically be used within a periodic task that takes measurements that need + * to be filtered, like speed or position values. + */ +class LowpassFilter { + + public: + + LowpassFilter(); + virtual ~LowpassFilter(); + void reset(); + void reset(float value); + void setPeriod(float period); + void setFrequency(float frequency); + float getFrequency(); + float filter(float value); + + private: + + float period; + float frequency; + float a11, a12, a21, a22, b1, b2; + float x1, x2; +}; + +#endif /* LOWPASS_FILTER_H_ */
--- a/Roboter.cpp Tue Apr 25 12:26:04 2017 +0000 +++ b/Roboter.cpp Tue May 09 13:21:49 2017 +0000 @@ -1,7 +1,7 @@ #include "Roboter.h" -Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2) +Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver) { @@ -14,65 +14,242 @@ this->pwmR = pwmR; this->pwmL = pwmL; - this->s1 = s1; - this->s2 = s2; + 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() +float Roboter::readSensor1() // Sensoren auslesen { return sensors[0].read(); } -void Roboter::bandeAusweichen() +void Roboter::bandeAusweichen() // Hindernissen ausweichen { - float offsetDir; - float offsetLin; - float x=0.13f; // Distanz ab welcher sensoren reagieren sollen - offsetDir = 0; - offsetLin = 0; - while(sensors[0] < x && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten - offsetLin = 0.1f; - *pwmL = 0.5f+offsetDir-offsetLin; // Setzt die Duty-Cycle auf 50% - *pwmR = 0.5f+offsetDir+offsetLin; + desiredSpeedLeft = -v; + desiredSpeedRight = v; } - while(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links - offsetDir = -0.05; - *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50% - *pwmR = 0.5f+offsetDir-offsetLin; + 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 - offsetDir = -0.05; - *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50% - *pwmR = 0.5f+offsetDir-offsetLin; + desiredSpeedLeft = -v; + desiredSpeedRight = -v; } while(sensors[5] < x && sensors[1]>(x+0.02f)) { // sensor links, roboter dreht nach rechts - offsetDir = 0.08; - *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50% - *pwmR = 0.5f+offsetDir-offsetLin; + 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::pickup () +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; +}
--- a/Roboter.h Tue Apr 25 12:26:04 2017 +0000 +++ b/Roboter.h Tue May 09 13:21:49 2017 +0000 @@ -5,37 +5,72 @@ #include "IRSensor.h" #include "Servo.h" #include "readCamera.h" +#include "EncoderCounter.h" +#include "LowpassFilter.h" + class Roboter { public: - Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut * pwmL, PwmOut* pwmR, Servo* s1, Servo* s2); + Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver); void bandeAusweichen(); float readSensor1(); - void pickup(); + int pickup(); + void anfahren(float cam); + int geradeFahren(); + int kreisFahren(); + int stateAuswahl(float cam, int temp); + void initSpeedcontrol(); + void speedCtrl(); + static const float PERIOD = 0.001f; // period of control task, given in [s] + static const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter + static const float LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s] + static const float KN = 40.0f; // speed constant of motor, given in [rpm/V] + static const float KP = 0.2f; // speed controller gain, given in [V/rpm] + static const float MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V] + static const float MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%) + static const float MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%) + static const float correction = 0.97f; // Korrekturfaktor für speedControl + static const float v = 20.0f; // Min. Geschw. + + private: + IRSensor sensors[6]; - PwmOut* pwmL; PwmOut* pwmR; - Servo*s1; - Servo*s2; - DigitalIn * switch1; - DigitalIn* switch2; + Servo* servo1; + Servo* servo2; + +//speed control------------------ + + EncoderCounter* counterRight; + EncoderCounter* counterLeft; + DigitalOut* enableMotorDriver; + + short previousValueCounterRight; + short previousValueCounterLeft; + float desiredSpeedLeft; + float desiredSpeedRight; + float actualSpeedLeft; + float actualSpeedRight; + LowpassFilter speedLeftFilter; + LowpassFilter speedRightFilter; + Ticker t1; + +//speed control------------------ }; - - #endif \ No newline at end of file
--- a/Servo.lib Tue Apr 25 12:26:04 2017 +0000 +++ b/Servo.lib Tue May 09 13:21:49 2017 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/simon/code/Servo/#36b69a7ced07 +https://developer.mbed.org/teams/Gruppe-3/code/Servo/#c29d0c32fe0a
--- a/main.cpp Tue Apr 25 12:26:04 2017 +0000 +++ b/main.cpp Tue May 09 13:21:49 2017 +0000 @@ -1,7 +1,11 @@ #include <mbed.h> #include "Roboter.h" -DigitalOut led(LED1); + +//Periphery for Encoder +EncoderCounter counterLeft(PB_6, PB_7); +EncoderCounter counterRight(PA_6, PC_7); + //Periphery for distance sensors AnalogIn distance(PB_1); @@ -12,11 +16,10 @@ IRSensor sensors[6]; // Periphery for servos -Servo servos1(PB_7); -Servo servos2(PA_6); +Servo servo1(PB_13); //Greiferservo Anschluss D2 +Servo servo2(PA_10); //Armservo Anschluss D0 //Periphery for Switch ON/OFF - DigitalIn switchOnOff(USER_BUTTON); //motor stuff @@ -27,124 +30,74 @@ //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); +// Erstellen eines Roboterobjekts +Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servo1, &servo2, &counterLeft, &counterRight, &enableMotorDriver); 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); - pwmL = 0.5f; - pwmR = 0.5f; - - - int state = 0; // Diese Variable gibt an in welchem State man sich befindet - int tempState = 2; - int time1 = 0; - int time2 = 0; + int state = 0; // Diese Variable gibt an in welchem State man sich befindet + int tempState = 2; // Wird benötig um zwischen Kameraauswertung und fahren zu wechseln + enable = 1; // Sensoren einschalten + float camValue; + servo1 = 0.0f; // Startpos. Greifer + servo2 = 0.0f; // Startpos. Arm + //wait(1); while(1) { - int camValue = readCamera(); +/* + + //printf("%d\n", state); switch(state) { case 0: // Roboter Anschalten mit Knopf printf("0"); - if (switchOnOff == 0) { - state = 1; + if (switchOnOff == 0) { // Bei Betätigung des Userbuttons wird das Programm gestartet + state = 5; } break; case 1: - printf("1"); - if(camValue == 1 || camValue == 2 || camValue == 3) { // Die Kamera erkennt ein grünen Klotz - state = 4; - } - if(camValue == 4) { // Roboter in Position - state = 5; - } - if(camValue == 0){ - state = tempState; - } + //printf("1"); + camValue = readCamera(); + state = roboter1.stateAuswahl(camValue, tempState); // Case-Auswahl aufgrund von der Kamera 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; - } + //printf("2"); + state = roboter1.kreisFahren(); // Im Kreis fahren + tempState = 2; 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; - } + //printf("3"); + roboter1.bandeAusweichen(); // Hindernissen ausweichen + state = roboter1.geradeFahren(); // Gerade Fahren + tempState = 3; break; - case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren - printf("4"); - roboter1.bandeAusweichen(); - if(camValue == 1) { // links fahren - pwmL = 0.45f; - pwmR = 0.45f; - } - if(camValue == 2) { // rechts fahren - pwmL = 0.55f; - pwmR = 0.55f; - } - if(camValue == 3) { // gerade fahren - pwmL = 0.55f; - pwmR = 0.45f; - } + case 4: + //printf("4"); + roboter1.bandeAusweichen(); // Hindernissen ausweichen + roboter1.anfahren(camValue); // Roboter erkennt gründer Klotz, Klotz wird angefahren state = 1; - break; - case 5: // Aufnehmen des Klotzes - printf("5"); - pwmL = 0.5f; - pwmR = 0.5f; - state = 1; - time1 = 0; - time2 = 0; - + case 5: + //printf("5"); + state = roboter1.pickup(); // Aufnehmen des Klotzes break; - default: break; - } - - //wait(0.1f); + */ + wait(0.01f); } - } -
--- a/mbed.bld Tue Apr 25 12:26:04 2017 +0000 +++ b/mbed.bld Tue May 09 13:21:49 2017 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90 \ No newline at end of file +https://mbed.org/users/mbed_official/code/mbed/builds/794e51388b66 \ No newline at end of file
--- a/readCamera.cpp Tue Apr 25 12:26:04 2017 +0000 +++ b/readCamera.cpp Tue May 09 13:21:49 2017 +0000 @@ -1,68 +1,73 @@ #include "readCamera.h" - Pixy pixy(I2C_SDA, I2C_SCL); Serial pc(SERIAL_TX, SERIAL_RX); static int i = 0; static int k = 0; -static int range = 1; // 1: fährt strahl an, 2: fährt kegel an -static int state = 0; +static float distance = 0; // für dynamische Geschwindigkeit +static float yLimit = 165; // Punkt an dem Roboter anhalten soll int j; uint16_t blocks; +float state; int readCamera() { pixy.setSerialOutput(&pc); + blocks = pixy.getBlocks(1); - while (1) { - blocks = pixy.getBlocks(1); + if (blocks) { + i++; + if (i % 5 == 0) { // Jedes 5. Bild wird ausgewertet + for (j = 0; j < blocks; j++) { + float xAxis = pixy.blocks[j].x; // Auswertung der x-Koordinate + float yAxis = pixy.blocks[j].y; // Auswertung der y-Koordinate + printf("%f y\n", yAxis); + //printf("%f x\n", xAxis); + + if(yAxis < yLimit && yAxis > 30) { + + if (xAxis < 130) { // links fahren + distance = (129 - xAxis)*(99.0/129.0); + state = 100 + distance; + + } else if (xAxis > 190) { // rechts fahren + distance = (xAxis - 191)*(99.0/128.0); + state = 200 + distance; + + } else { // fahre gerade + distance = ((yAxis - yLimit) / yLimit)*(-99); + state = 300 + distance; + } + } - if (blocks) { - i++; - if (i % 5 == 0) { - for (j = 0; j < blocks; j++) { - int xAxis = pixy.blocks[j].x; - int yAxis = pixy.blocks[j].y; - - - - // soll drehen bis in stahl, fahren bis kegel, 25 pixel links rechts kegel - if(xAxis < 185 && xAxis > 135 && yAxis > 155 && yAxis < 190 ) { // wenn Klotz an Position zum aufnehmen - state = 4; - range = 1; - } else { - if (range == 1){ // fährt Strahl an - if (xAxis < 155){ - state = 1; - } else if (xAxis > 165){ - state = 2; - } else{ // fährt gerade - state = 3; - range = 2; - } - } else { // fährt solange bis Kegelrand - if (xAxis > 185 || xAxis < 135) { - range = 1; - } - } + if(yAxis > yLimit) { + + if (xAxis < 145) { // links fahren + state = 100; + + } else if (xAxis > 175) { // rechts fahren + state = 200; + + } else { // Klotz bereit zum Aufnehmen + state = 400; } - - - } - k=0; + if(yAxis <= 30) { // Zone mit vielen Fehlern + state = 0; // deshalb wird alles, was dort drin ist als "kein Klotz in Sichtweite"" gewertet + } } - } else{ - k++; - if (k % 20 == 0) { - i=0; - state = 0; - } + k = 0; + i = 0; } - //printf("%d\n\r",state); - //printf("%d, %d\n\r", xAxis, yAxis); - return state; + } else { + k++; + if (k % 20 == 0) { + i=0; + state = 0; // kein Klotz in Sichtweite + } } + //printf("%f\n\r",state); + return state; } \ No newline at end of file
--- a/readCamera.h Tue Apr 25 12:26:04 2017 +0000 +++ b/readCamera.h Tue May 09 13:21:49 2017 +0000 @@ -6,11 +6,9 @@ #include "PixyLink.h" #include "PixyLinkI2C.h" + int readCamera(); - - - #endif \ No newline at end of file