Encoder und Sensorwerte
Dependencies: mbed
Fork of Bewegungen by
Revision 1:d40ff07e2fe0, committed 2017-05-10
- Comitter:
- EHess
- Date:
- Wed May 10 09:14:12 2017 +0000
- Parent:
- 0:96f88638114b
- Commit message:
- Encoder und Sensorwerte
Changed in this revision
diff -r 96f88638114b -r d40ff07e2fe0 IRSensor.cpp --- a/IRSensor.cpp Tue Mar 21 14:57:54 2017 +0000 +++ b/IRSensor.cpp Wed May 10 09:14:12 2017 +0000 @@ -1,67 +1,43 @@ -/* - * IRSensor.cpp - * Copyright (c) 2016, ZHAW - * All rights reserved. - */ +#include "IRSensor.h" +#include <cmath> -#include <cmath> -#include "IRSensor.h" - +//E. Hess +//IRSensor.cpp -/** - * Creates an IRSensor object. - * @param distance an analog input object to read the voltage of the sensor. - * @param bit0 a digital output to set the first bit of the multiplexer. - * @param bit1 a digital output to set the second bit of the multiplexer. - * @param bit2 a digital output to set the third bit of the multiplexer. - * @param number the number of the sensor, either 0, 1, 2, 3, 4 or 5. - */ -IRSensor::IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) -{ - init(distance, bit0, bit1, bit2, number); -} +//Konstruktor -> Erstellt ein IRSensor-Objekt +//AnalogIn* distance -> Liest die Voltanzahl des Sensors aus +//DigitalOut* bit0, bit1, bit2 -> Binär-Outputs wählen die 6 Sensoren an +//Int number -> Sensornummer als int (0-5) - -IRSensor::IRSensor() -{ -} +IRSensor::IRSensor() {} -void IRSensor::init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) -{ - - this->distance = distance; // set local references to objects +IRSensor::IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) { + this->distance = distance; //Weist den Objektvariablen, die eingegebenen Werte zu this->bit0 = bit0; this->bit1 = bit1; this->bit2 = bit2; - this->number = number; } - -/** - * Deletes the IRSensor object. - */ +//Destruktor -> Löscht das IRSensor-Objekt IRSensor::~IRSensor() {} -/** - * Gets the distance measured with the IR sensor in [m]. - * @return the distance, given in [m]. - */ +//Initialisiert nachträglich +void IRSensor::init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) { + this->distance = distance; //Weist den Objektvariablen, die eingegebenen Werte zu + this->bit0 = bit0; + this->bit1 = bit1; + this->bit2 = bit2; + this->number = number; +} + +//Distanzrechner float IRSensor::read() { - *bit0 = (number >> 0) & 1; - *bit1 = (number >> 1) & 1; - *bit2 = (number >> 2) & 1; - - float d = -0.38f*sqrt(distance->read())+0.38f; // calculate the distance in [m] - return d; -} + *bit0 = (number >> 0) & 1; //Vergleicht das Least-Significant-Bit von number mit 1 und setzt *bit0 (z.B. 5: 0000'0101 & 0000'0001 == 0000'0001) + *bit1 = (number >> 1) & 1; //Vergleicht das zweite Bit von rechts + *bit2 = (number >> 2) & 1; //Vergleicht das dritte Bit von rechts -/** - * The empty operator is a shorthand notation of the <code>read()</code> method. - */ -IRSensor::operator float() -{ - - return read(); -} + float d = distance->read(); + return d; +} \ No newline at end of file
diff -r 96f88638114b -r d40ff07e2fe0 IRSensor.h --- a/IRSensor.h Tue Mar 21 14:57:54 2017 +0000 +++ b/IRSensor.h Wed May 10 09:14:12 2017 +0000 @@ -1,40 +1,25 @@ -/* - * IRSensor.h - * Copyright (c) 2016, ZHAW - * All rights reserved. - */ - -#ifndef IR_SENSOR_H_ -#define IR_SENSOR_H_ +#ifndef IR_SENSOR_H +#define IR_SENSOR_H -#include <cstdlib> -#include <mbed.h> +#include "mbed.h" -/** - * This is a device driver class to read the distance measured with a Sharp IR sensor. - */ -class IRSensor -{ +//E. Hess +//IRSensor.h -public: - - IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number); - IRSensor(); - - void init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number); - virtual ~IRSensor(); - float read(); +class IRSensor { + public: + IRSensor(); + IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number); + ~IRSensor(); + void init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number); + float read(); - operator float(); - -private: - - AnalogIn* distance; - DigitalOut* bit0; - DigitalOut* bit1; - DigitalOut* bit2; - - int number; + private: + AnalogIn* distance; + DigitalOut* bit0; + DigitalOut* bit1; + DigitalOut* bit2; + int number; }; -#endif /* IR_SENSOR_H_ */ +#endif \ No newline at end of file
diff -r 96f88638114b -r d40ff07e2fe0 LowpassFilter.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LowpassFilter.cpp Wed May 10 09:14:12 2017 +0000 @@ -0,0 +1,107 @@ +#include "LowpassFilter.h" + +//E. Hess +//LowpassFilter.cpp + +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; +}
diff -r 96f88638114b -r d40ff07e2fe0 LowpassFilter.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LowpassFilter.h Wed May 10 09:14:12 2017 +0000 @@ -0,0 +1,36 @@ +#ifndef LOWPASS_FILTER_H_ +#define LOWPASS_FILTER_H_ + +#include <cstdlib> +#include <cmath> + +//E. Hess +//LowpassFilter.cpp + +/** + * 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
diff -r 96f88638114b -r d40ff07e2fe0 MotorEncoder.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorEncoder.cpp Wed May 10 09:14:12 2017 +0000 @@ -0,0 +1,115 @@ +#include "MotorEncoder.h" + +//E. Hess +//MotorEncoder.h + +using namespace std; + +// Erstellt einen Treiber um den Encoder zu lesen und initialisiert ihn +// PinName a ist der Input für Kanal A +// PinName b ist der Input für Kanal B + +MotorEncoder::MotorEncoder(PinName a, PinName b) { + // Überprüft die Inputs + if ((a == PA_6) && (b == PC_7)) { // Rechter Motor + // pinmap OK for TIM3 CH1 and CH2 + TIM = TIM3; + + // Kunfiguration vom Reset und der Taktsteuerung + 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)) { // Links Motor + + // 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 konnte nicht gefunden werden!\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 +} + +MotorEncoder::~MotorEncoder() {} + +// Setzt den Zähler auf NULL +void MotorEncoder::reset() { + + TIM->CNT = 0x0000; +} + +// Setzt den Zähler auf NULL zurück +void MotorEncoder::reset(short offset) { + TIM->CNT = -offset; +} + +// Liest den Zähler +short MotorEncoder::read() { + return (short)(-TIM->CNT); +} + +// Kurznotation für read() +MotorEncoder::operator short() { + return read(); +}
diff -r 96f88638114b -r d40ff07e2fe0 MotorEncoder.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorEncoder.h Wed May 10 09:14:12 2017 +0000 @@ -0,0 +1,23 @@ +#ifndef MOTOR_ENCODER_H +#define MOTOR_ENCODER_H + +#include <cstdlib> +#include <mbed.h> + +//E. Hess +//MotorEncoder.h + +class MotorEncoder { + public: + MotorEncoder(PinName a, PinName b); + virtual ~MotorEncoder(); + void reset(); + void reset(short offset); + short read(); + operator short(); + + private: + TIM_TypeDef* TIM; +}; + +#endif \ No newline at end of file
diff -r 96f88638114b -r d40ff07e2fe0 main.cpp --- a/main.cpp Tue Mar 21 14:57:54 2017 +0000 +++ b/main.cpp Wed May 10 09:14:12 2017 +0000 @@ -1,75 +1,144 @@ #include "mbed.h" #include "IRSensor.h" - -//E. Hess -//Robotterbewegungen +#include "MotorEncoder.h" +#include "LowpassFilter.h" -DigitalOut led(LED1); +DigitalOut led(LED1); //Zustands-LED: Grüne LED für Benutzer -//Erstellt In- / Outputs -AnalogIn distance(PB_1); -DigitalOut enable(PC_1); -DigitalOut bit0(PH_1); +AnalogIn distance(PB_1); //Input der Distanzsensoren +DigitalOut enableSensor(PC_1); //Aktivierung der IRSensoren +DigitalOut bit0(PH_1); //Ansteuerung der Sensoren 0-5 mit 3 Bits DigitalOut bit1(PC_2); DigitalOut bit2(PC_3); -IRSensor sensors[6]; //-> Was +IRSensor sensors[6]; //Erstellt 6 IRSensor-Objekte als Array -//LED Indikatoren rund um Roboter -DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; - -//Timer-Objekt für LED- und Distanzsensor -Ticker t1; +DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; //LED-Outputs der Sensoren -//Motoren -DigitalOut enableMotorDriver(PB_2); //Erstellt das Objekt -PwmOut pwmL(PA_8); -PwmOut pwmR(PA_9); -//DistanzLEDs ->Was -void ledDistance(){ - for( int ii = 0; ii<6; ++ii) - sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0; +//Titel printf() +void title() { + printf("\f < \t\t - \t\t >\n\r"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite } -//Blinkt beim Start und startet die DistanzLEDs -void ledShow(){ - static int timer = 0; - for( int ii = 0; ii<6; ++ii) - leds[ii] = !leds[ii]; + +const float PERIOD = 0.002f; // period of control task, given in [s] +const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter +const float LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s] +const float KN = 40.0f; // speed constant of motor, given in [rpm/V] +const float KP = 0.2f; // speed controller gain, given in [V/rpm] +const float MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V] +const float MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%) +const float MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%) + +MotorEncoder counterLeft(PB_6, PB_7); +MotorEncoder counterRight(PA_6, PC_7); + +LowpassFilter speedLeftFilter; +LowpassFilter speedRightFilter; + +DigitalOut enableMotorDriver(PB_2); +PwmOut pwmLeft(PA_8); +PwmOut pwmRight(PA_9); + +DigitalOut my_led(LED1); + +short previousValueCounterRight = 0; +short previousValueCounterLeft = 0; + +float desiredSpeedLeft; +float desiredSpeedRight; - //Beendet den Ticker und startet die DistanzLED-Show - if( ++timer > 10) { - t1.detach(); - t1.attach( &ledDistance, 0.01f ); - } +float actualSpeedLeft; +float actualSpeedRight; + +void 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-actualSpeedRight)+desiredSpeedRight/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; + + pwmLeft = 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; + + pwmRight = dutyCycleRight; } -int main() -{ - pwmL.period(0.00005f); // Setzt die Periode auf 50 μs -> f speichert float anstatt double (schneller) - pwmR.period(0.00005f); - pwmL = 0.5f; // Setzt die Duty-Cycle auf 50% - pwmR = 0.5f; - enableMotorDriver = 1; +int main(){ + + // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us + pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs + pwmRight.period(0.00005f); + pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us + pwmRight = 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); - t1.attach( &ledShow, 0.05f ); //->Was + desiredSpeedLeft = 0.0f; + desiredSpeedRight = 0.0f; + actualSpeedLeft = 0.0f; + actualSpeedRight = 0.0f; + + Ticker t1; + t1.attach( &speedCtrl, PERIOD); - //Initialisiert Distanzsensoren - for( int ii = 0; ii<6; ++ii) - sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii); + desiredSpeedLeft = 15.0f; //50 RPM + desiredSpeedRight = 15.0f; //50 RPM + enableMotorDriver = 1; + + float sensorMittelwert[6]; //Array der 6 Sensorenwerte + float sensorTiefbass[6]; + int zaehler = 0; - enable = 1; + //Initialisiert Distanzsensoren und setzt sensorValue und sensorTiefbass auf NULL + for( int i = 0; i < 6; i++) { + sensors[i].init(&distance, &bit0, &bit1, &bit2, i); + sensorMittelwert[i] = 0.0f; + sensorTiefbass[i] = 0.0f; + } + enableSensor = 1; //Aktiviert die IRSensoren while(1) { - wait( 0.2f ); - if(sensors[0].read() < 0.2f){ - pwmL = 0.55f; - pwmR = 0.55f; + for(int j = 0; j < 25; j++){ //Zählt 25 Sensorwerten pro Sensor zusammen + for(int i = 0; i < 6; i++){ + sensorMittelwert[i] += sensors[i].read(); + } + wait( 0.001f ); + } + for(int i = 0; i < 6; i++){ + sensorTiefbass[i] = sensorTiefbass[i]*0.75f + sensorMittelwert[i]*0.25f; //Verrechnet den neuen Wert mit dem alten + sensorMittelwert[i] = 0.0f; //Setzt die Sensorwerte auf NULL } - else{ - pwmL = 0.6f; - pwmR = 0.4f; + + printf("%f\t%f\t%f\n\r", sensorTiefbass[5], sensorTiefbass[0], sensorTiefbass[1]); //Plottet die unteren Sensoren + Mitte-Oben + + zaehler++; + if(zaehler % 120 == 0){ + wait(3.5f); } -//float x = sensor[0]; + if(zaehler % 40 == 0) title(); //Erstellt nach 40 Zeilen eine neue Seite } -} +} \ No newline at end of file