ARDUINO SEGWAY
Hello!
More than 3 years ago I started to tinker a homemade segway. Though it's still not really finished (I have to replace the 250W Motors by 500W-models) my children are able to drive around.
If you're going to build one you will Need the following parts:
* wheelset: In my case I use 12 1/2 x 2.75 tires usually taken for dirt bikes (f.e. www.amazon.de/HMParts-REIFEN-MIT-SCHLAUCH-12/dp/B0038X9ODA; http://www.ebay.com/itm/Tire-Rear-Wheel-Mini-Pocket-Dirt-Bike-47cc-49cc-50cc-COOLSTER-QG-50-RX1-Traxxis-/182346539856?hash=item2a74b36350:g:BxUAAOSwh6xTtmwX&vxp=mtr)
* batteries: I use two 12V/12Ah lead accumulators to get 24V for the Motors
* Motors: At the moment two 24V/250W models but the 500W-motors are already waiting for their use
* for the axle I asked a local welder and some of my bicycle-parts (bar, stem) fit perfect too.
Step 1: The Motor Driver
First I tried a homemade full-bridge. To control it I used the analog-out (pwm) of the arduino. But with this variant I didn't succeed. Therefore I bought a commercial product, a 2x60A Motor Driver from sabertooth (www.dimensionengineering.com/products/sabertooth2x60).
The arduino is able to communicate with the sabertooth via a simplified serial code.
Step 2: The Angle Determination
To determinate the current angle I first used a separate accelerometer (ADXL335) and gyroscope (LY530AL). Last week I changed this Setup and the two boards have been replaced by the MPU6050.sensor, which combines the two elements.
The advantage of the gyroscope is, that accelerations don't influence the measurements. The disadvantage is, that the gyro has a drift and after a while the angle goes higher and higher.
The Advantage of the accelerometer is, that it doesn't show any drift like the gyro. But it's being influenced by accelerations, which change the Output.
A filter combines both pro's so the calculated angle is near the real value. This filter can be a simple complementary-filter (like in the Picture) or the more complicated kalman-filter. The Input of the kalman-filter is the actual angle measured by the accelerometer, the angular Speed measured by the gyroscope and the time-step between two measurements. With these Parameters the kalman-filter is able to calculate an angle, which follows the gyro in times of high accelerations and in times of less accelerations it approaches the value of the accelerometer. In the Picture you can see the drift of the gyro and the good Job of the filter.
Step 3: The PID-controller
The segway tries to get the angle down to 0°. If the angle is positive, the segway brakes or accelerates backwards. If it's negative the segway accelerates forward.
But which signal get the motors? This is exactly the task of the pid-Controller.
The proportional-part: The angle should be 0°. So the greater the angle, the higher the speed of the motors.
Mathematically you can write:
angle_error = angle_should - angle_is
p-Motor-value = k_p * angle_error
The integral-part: When the current angle isn't zero, the Controller tries to get the segway horizontally again. But this approach shouldn't be slow and the smaller the angle-values, the slower the changes. Therefore the Controller allows a faster approach with the disadvantage, that the movement doesn't stop at 0° and goes a bit to the other side. This behaviour is guaranteed by the integral-part. All the angles are added (integrated) and this integral over time should be Zero. So if the angle is f.e. positive for a while, the Controller tries to get the seqway into the negative area. This guaranteeds faster approaches...
Mathematically you can write:
error_sum_new = error_sum_old + error
i-motor-value = k_i * (error_sum_new)
error_sum_old = error_sum_new
The derivative-part: Not just the current angle is important but also the changes of the angle. Let me explain: If the angle is f.e. 10° and it doesn't Change a lot, the Motors don't need to correct this shift very fast. But if the angle is 10° and it changes very fast (high derivative), than the motors have to correct this also very fast.
Mathematically you can write:
d-motor-value = k_d * angular Speed
Finally you get: motor-value = p-motor-value + i-motor-value + d-motor-value
Step 4: The Complete Setup
Now it's time to start the segway. To drive not only straight ahead you have to mount two buttons. When you push f.e. the right one, the left Motor increases Speed and the right Motor slows down. To ensure smooth transitions the added and deducted value starts at zero and increases step by step (+0.5 every loop) until he reaches the maximum value (f.e. 30).
To try out your own segway I 've attached the simple program for you. Good luck and don't follow the segway-inventor James Heselden down a cliff ;-)
Maybe you want to take a look at my YouTube-channel: www.youtube.com/user/stopperl16/Videos
// -------------------------------------------------------------------------------------------------------------------------------------------------------------------------
// ------------ Programm zur Berechnung des Neigungswinkels aus den Gyro- und Beschleunigungsdaten unter Verwendung eines Kalman-Filters ----------------------------------
// -------------------------- und PID-Regelung zur seriellen Ansteuerung der beiden Motoren mittels Sabertooth-Motortreiber -----------------------------------------------
// -------------------------------------------------------------------------------------------------------------------------------------------------------------------------
#include "Wire.h"
#include "I2Cdev.h" // I2Cdev and MPU6050 must be installed as libraries
#include "MPU6050.h" // class default I2C address is 0x68 = AD0 low
#include <math.h>
#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>
SoftwareSerial SWSerial(NOT_A_PIN, 11); // RX on no pin (unused), TX on pin 11 (to S1).
SabertoothSimplified ST(SWSerial); // Use SWSerial as the serial port.
MPU6050 accelgyro;
int16_t ax, ay, az; // Beschleunigungswerte in x-,y- und z-Richtung des MPU6050-Sensors
int16_t gx, gy, gz; // Winkelgeschwindigkeitswerte in x-,y- und z-Richtung des MPU6050-Sensors
#define Pin_Lenkung_rechts 12 // Pin-Anschluss für den Lenkbefehl Rechts
#define Pin_Lenkung_links 13 // Pin-Anschluss für den Lenkbefehl Links
#define Pin_Schalter_Box 3 // Pin-Anschluss des Box-Schalters, um zwischen Motorsynchronisation und I-Regelung zu wählen
#define Pin_PID_Regelung_P A1 // Pin-Anschluss für das Poti zur Veränderung des P-Anteils
#define Pin_PID_Regelung_I A2 // Pin-Anschluss für das Poti zur Veränderung des I-Anteils
#define Pin_PID_Regelung_D A3 // Pin-Anschluss für das Poti zur Veränderung des D-Anteils
int LoopTime_Soll = 9; // gewünschte Schleifendauer in ms um auf die 100 Hz zu gelangen
int LoopTime_Angepasst = LoopTime_Soll; // letzte Schleifenzeit mit erzwungener Pause
int LoopTime_Bisher = LoopTime_Soll; // letzte Schleifenzeit ohne erzwungener Pause
unsigned long LoopTime_Start = 0; // Startzeit der Schleife
float Winkel; // aktueller Neigungswinkel
float Winkel_Soll; // Sollwert des Neigungswinkels, also 0°
float Winkel_Grenze; // maximal erlaubter Neigungswinkel, über dem das Segway abgeschaltet wird
float ACC_angle; // Winkel vom Beschleunigungssensor
float GYRO_rate; // Winkelgeschwindigkeit vom Gyro-Sensor
float Kp,Ki,Kd,K; // Konstanten zur PID-Regelung, Differentteil, Integralteil, Differentialteil, Gesamtanteil
int Motor; // von der PID-Regelung erhaltener Wert zur Motoransteuerung
int Motor_rechts, Motor_links; // Werte für die beiden Motoren
float K_Motor_links, K_Motor_rechts; // Korrekturfaktoren für einen synchronen Lauf der beiden Motoren
int Schalter_Box; // Variable, welche die Schalterstellung an der Box abfragt
int Lenkung_Eingang_rechts = 0; // Variable zum Erfassen eines Lenkbefehls nach Rechts
int Lenkung_Eingang_links = 0; // Variable zum Erfassen eines Lenkbefehls nach Links
float Lenkung_max; // Wert, um den sich die Motoransteuerung bei einem Lenkbefehl maximal ändern soll
float Lenkung_rechts, Lenkung_links; // aktueller und schrittweise erhöhter Ansteuerungswert beim Lenken nach rechts bzw. links
// ****************************************************************************
// ****************************** SETUP ***************************************
// ****************************************************************************
void setup()
{
Wire.begin();
//SWSerial.begin(9600); // This is the baud rate you chose with the DIP switches.
Serial.begin(9600); // baud rate für den seriellen Monitor, um die Werte zu überprüfen
// initialize device
accelgyro.initialize();
calibrateSensors(); // Unterprogramm zum einmaligen Kalibrieren der Sensoren
}
// ***********************************************************************************
// ****************************** Kalibrierung ***************************************
// ***********************************************************************************
void calibrateSensors() // einmalige Ermittlung der Sensor-Nullwerte (Mittelwert aus jeweils 50 Messungen)
{
// =================================================
// ======== Änderung der Sensor-Auflösung ==========
// =================================================
// ===========================================================================
// read raw accel/gyro measurements from device
// Ausgabewerte-Acc: Auflösung 2g: 16384/g Auflösung 4g: 8192/g
// Ausgabewerte-Gyro: Auflösung 250°/s: 131/°/s Auflösung 500°/s: 65.5/°/s
// ===========================================================================
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_500);
// Versuch, das "Aufheulen" der beiden Motoren zu Beginn zu vermeiden
ST.motor(1, 0);
ST.motor(2, 0);
Winkel_Soll = 0.0; // Sollwert für den Neigungswinkel
Winkel_Grenze = 30.0; // maximal erlaubter Neigungswinkel
// *******************************************************
// ********** K - Werte für die PID-Regelung *************
// *******************************************************
Kp = analogRead(Pin_PID_Regelung_P) * 25.0 / 1023.0; // Differenzenanteil mit Poti festgelegt
Ki = 0.1; // Integralanteil mit Poti festgelegt (Schalter aber womöglich auf Motorkorrektur, daher mit 0.1 zunächst festgesetzt)
Kd = analogRead(Pin_PID_Regelung_D) * 100.0 / 1023.0; // Differentialanteil mit Poti festgelegt
K = 1.0; // Gesamtanteil
// **************************************************
// ********** K - Werte für die Motoren *************
// **************************************************
pinMode(Pin_Schalter_Box, INPUT); // Pin für die Auswahl zwischen I-Regelung und Motorsynchronisation
K_Motor_rechts = 1.0; // Korrekturfaktor für den rechten Motor
K_Motor_links = 0.8; // Korrekturfaktor für den linken Motor
// **********************************************
// ********** Werte für die Lenkung *************
// **********************************************
Lenkung_max = 25.0; // Wert, um den sich die Motoransteuerung bei einem Lenkbefehl MAXIMAL ändern soll
Lenkung_rechts = 0.0; // aktueller Zusatzwert beim Lenkvorgang nach rechts
Lenkung_links = 0.0; // aktueller Zusatzwert beim Lenkvorgang nach links
pinMode(Pin_Lenkung_rechts, INPUT); // Pin für Lenkung nach Rechts wird als Input deklariert
pinMode(Pin_Lenkung_links, INPUT); // Pin für Lenkung nach Links wird als Input deklariert
}
// ***************************************************************************************************************************************************
// ***************************************************************************************************************************************************
// ********************************************************************** HAUPTSCHLEIFE **************************************************************
// ***************************************************************************************************************************************************
// ***************************************************************************************************************************************************
void loop()
{
// *******************************************************
// ********************* Sensorabfrage *******************
// *******************************************************
// ===========================================================================
// read raw accel/gyro measurements from device
// Ausgabewerte-Acc: Auflösung 2g: 16384/g Auflösung 4g: 8192/g
// Ausgabewerte-Gyro: Auflösung 250°/s: 131/°/s Auflösung 500°/s: 65.5/°/s
// ===========================================================================
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ACC_angle = atan(ay * 1.0 / az * 1.0) * 180.0 / 3.141592654; // Auflösung 2g: 16384/g
//ACC_angle = atan((ay/16384.0) / (az/16384.0)) * 180.0 / 3.141592654; // Auflösung 2g: 16384/g
GYRO_rate = gx/65.5; // Auflösung 500°/s: 65.5/°/s
// *******************************************************
// ********** K - Werte für die PID-Regelung *************
// *******************************************************
Kp = analogRead(Pin_PID_Regelung_P) * 25.0 / 1023.0; // Differenzenanteil mit Poti festgelegt; Maximum = 25
Kd = analogRead(Pin_PID_Regelung_D) * 100.0 / 1023.0; // Differentialanteil mit Poti festgelegt; Maximum = 100
Schalter_Box = digitalRead(Pin_Schalter_Box); // Abfrage des Pins für den Schalterzustand an der Box
if (Schalter_Box == HIGH) // Mittels Schalter an der Box I-Regelung aktiviert
{
Ki = analogRead(Pin_PID_Regelung_I) * 2.0 / 1023.0; // Integralanteil mit Poti festgelegt; Maximum = 2
}
else // Mittels Schalter an der Box Motor-Regelung aktiviert
{
K_Motor_rechts = analogRead(Pin_PID_Regelung_I) * 2.0 / 1023.0; // Korrekturfaktor für einen synchronen Lauf der beiden Motoren; Maximum = 2
}
// ********************************************************************************
// ****************** Kalmanfilter, PWM-Berechnung und Motorwerte *****************
// ********************************************************************************
Winkel = kalmanCalculate(ACC_angle, GYRO_rate, LoopTime_Angepasst); // mit Kalmanfilter berechneter Winkel
if (Winkel > Winkel_Grenze || Winkel < (Winkel_Grenze * (-1)))
{
// ===============================================
// Abbruch aufgrund des zu großen Neigungswinkels!
// ===============================================
ST.motor(1, 0);
ST.motor(2, 0);
}
else
{
// =========================
// Neigungswinkel in Ordnung
// =========================
Motor = pid(Winkel, Winkel_Soll, GYRO_rate); // Berechnung des PWM-Werts zur Ansteuerung der Motoren
Motor_rechts = K_Motor_rechts * Motor; // Berechnung der mittels K-Faktor synchronisierten Motordrehzahl für den rechten Motor
Motor_links = K_Motor_links * Motor; // Berechnung der mittels K-Faktor synchronisierten Motordrehzahl für den linken Motor
// **************************************************************************************
// ***** Abfrage ob die Lenkung betätigt wurde und Veränderung der Motoransteuerung *****
// **************************************************************************************
Lenkung_Eingang_rechts = digitalRead(Pin_Lenkung_rechts); // Abfrage des Pins für Lenkung nach Rechts
if (Lenkung_Eingang_rechts == HIGH)
{
// ******************************************
// *** Lenkung nach Rechts wurde gedrückt ***
// ******************************************
if (Motor_rechts >= 0) // segway fährt gerade vorwärts oder steht. Welcher Motor abgefragt wird, ist egal.
{
Motor_rechts = Motor_rechts - (int)Lenkung_rechts; // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 - 0.1) versuchen
Motor_links = Motor_links + (int)Lenkung_rechts; // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 + 0.1)) versuchen
}
else // segway fährt gerade rückwärts
{
Motor_rechts = Motor_rechts + (int)Lenkung_rechts; // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 + 0.1) versuchen
Motor_links = Motor_links - (int)Lenkung_rechts; // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 - 0.1) versuchen
}
Lenkung_rechts = Lenkung_rechts + 0.05; // Besser nur um z.B. 0.1 pro Abfrage erhöhen, damit Lenkung nicht zu abrupt erfolgt!
if (Lenkung_rechts > Lenkung_max) Lenkung_rechts = Lenkung_max; // Maximaler Lenkungswert darf nicht überschritten werden!
//Lenkung_rechts = constrain(Lenkung_rechts, 0, Lenkung_max); // rechter Lenkungswert ins Intervall [0,Lenkung_max] gebracht
}
else
{
Lenkung_rechts = 0.0;
}
Lenkung_Eingang_links = digitalRead(Pin_Lenkung_links); // Abfrage des Pins für Lenkung nach Links
if (Lenkung_Eingang_links == HIGH)
{
// *****************************************
// *** Lenkung nach Links wurde gedrückt ***
// *****************************************
if (Motor_links >= 0) // segway fährt gerade vorwärts oder steht. Welcher Motor abgefragt wird ist egal.
{
Motor_rechts = Motor_rechts + (int)Lenkung_links; // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 + 0.1) versuchen
Motor_links = Motor_links - (int)Lenkung_links; // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 - 0.1) versuchen
}
else // segway fährt gerade rückwärts
{
Motor_rechts = Motor_rechts - (int)Lenkung_links; // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 - 0.1) versuchen
Motor_links = Motor_links + (int)Lenkung_links; // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 + 0.1) versuchen
}
Lenkung_links = Lenkung_links + 0.05; // Besser nur um z.B. 0.1 pro Abfrage erhöhen, damit Lenkung nicht zu abrupt erfolgt!
if (Lenkung_links > Lenkung_max) Lenkung_links = Lenkung_max; // Maximaler Lenkungswert darf nicht überschritten werden!
//Lenkung_links = constrain(Lenkung_links, 0, Lenkung_max); // linker Lenkungswert ins Intervall [0,Lenkung_max] gebracht
}
else
{
Lenkung_links = 0.0;
}
// *******************************************************************************************
// ******************************** Ansteuern der Motoren ***********************************
// *******************************************************************************************
Motor_rechts = constrain(Motor_rechts, -127, 127); // rechter Motorenwert ins Intervall [-127,127] gebracht
Motor_links = constrain(Motor_links, -127, 127); // linker Motorenwert ins Intervall [-127,127] gebracht
/*
// Gebrauch einer Wurzelfunktion anstelle der linearen Ansteuerfunktion zur Verbesserung des Ansprechverhaltens bei niedrigen Motorwerten
// ======================================================================================================================================
if (Motor_rechts >= 0) // rechter Motor dreht vorwärts
{
Motor_rechts = sqrt(127 * Motor_rechts); // zur Verbesserung des Ansprechverhaltens bei niedrigen Motorwerten
ST.motor(2, Motor_rechts);
}
else // rechter Motor dreht rückwärts
{
Motor_rechts = -sqrt(127 * -Motor_rechts); // zur Verbesserung des Ansprechverhaltens bei niedrigen Motorwerten
ST.motor(2, Motor_rechts);
}
if (Motor_links >= 0) // linker Motor dreht vorwärts
{
Motor_links = sqrt(127 * Motor_links); // zur Verbesserung des Ansprechverhaltens bei niedrigen Motorwerten
ST.motor(1, Motor_links);
}
else // linker Motor dreht rückwärts
{
Motor_links = -sqrt(127 * -Motor_links); // zur Verbesserung des Ansprechverhaltens bei niedrigen Motorwerten
ST.motor(1, Motor_links);
}
*/
ST.motor(1, Motor_links);
ST.motor(2, Motor_rechts);
}
// ************************************************************************
// *********************** Ausgabe der Messwerte **************************
// ************************************************************************
Werteausgabe();
// ******************************************************************
// *********************** Tastaturabfrage **************************
// ******************************************************************
// Tastatureingabe();
// **********************************************************************
// *********************** loop timing control **************************
// **********************************************************************
LoopTime_Bisher = millis() - LoopTime_Start; // Zeit seit der letzten Schleife
if(LoopTime_Bisher < LoopTime_Soll)
{
delay(LoopTime_Soll - LoopTime_Bisher); // Verzögerung, um die gleiche Schleifenzeit zu erhalten
}
LoopTime_Angepasst = millis() - LoopTime_Start; // aktualisierte Dauer der letzten Schleife, sollte gleich LoopTime_Soll = z.B. 10 msek sein!
LoopTime_Start = millis(); // neue Starteit der Schleife
}
// ********************************************************************************************
// ****************** Werteausgabe an die serielle Schnittstelle ******************************
// ********************************************************************************************
void Werteausgabe()
{
/*
Serial.print(Winkel);
Serial.print(" ");
Serial.println(Motor);
Serial.print(" ");
*/
Serial.print("a_y = ");
Serial.print(ay/16384.0);
Serial.print(" a_z = ");
Serial.print(az/16384.0);
Serial.print(" ACC_angle = ");
Serial.print(ACC_angle,0);
Serial.print(" GYRO_rate = ");
Serial.print(GYRO_rate,0);
Serial.print(" Winkel: ");
Serial.println(Winkel,0);
/*
Serial.print(" Motor: ");
Serial.print(Motor);
Serial.print(" Motor_rechts: ");
Serial.print(Motor_rechts);
Serial.print(" Motor_links: ");
Serial.println(Motor_links);
*/
}
// ******************************************************************************************************
// ***************************************** PID-Steuerung **********************************************
// ******************************************************************************************************
float error;
float last_error = 0;
float pTerm;
float iTerm;
float dTerm;
float integrated_error = 0;
int GUARD_GAIN = 40; // maximal aufintegrierter Winkelfehler
int pid(float Winkel_aktuell, float Winkel_Vorgabe, float Winkelgeschwindigkeit)
{
error = Winkel_Vorgabe - Winkel_aktuell;
pTerm = Kp * error; // Differenzenanteil
integrated_error = integrated_error + error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN); // Integralanteil
dTerm = Kd * Winkelgeschwindigkeit / 100.0; // Differentialanteil; :100 um auf brauchbare Werte zu kommen!
/*
Serial.print(" K_p: ");
Serial.print(pTerm);
Serial.print(" K_d: ");
Serial.println(dTerm);
*/
last_error = error;
// Serial.println(K*(pTerm + iTerm + dTerm));
return constrain(K*(pTerm + iTerm + dTerm), -127, 127); // Ausgabe des Motorwerts für die beiden Motoren innerhalb der Grenzen [-127,127]
}
// ******************************************************************************************************
// ************************************** Kalman filter module ******************************************
// ******************************************************************************************************
float Q_angle = 0.001; // E(alpha2) = 0.001
float Q_gyro = 0.003; // E(bias2) = 0.003
float R_angle = 0.001; // Sz = 0.03 !!! je größer die Zahl, desto unempfindlicher reagiert der Winkel auf Veränderungen !!!
float x_angle = 0;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, y, S;
float K_0, K_1;
float kalmanCalculate(float newAngle, float newRate, int looptime)
{
dt = float(looptime)/1000; // dt in Sekunden
x_angle = x_angle + dt * (newRate - x_bias);
P_00 = P_00 - dt * (P_10 + P_01) + Q_angle * dt;
P_01 = P_01 - dt * P_11;
P_10 = P_10 - dt * P_11;
P_11 = P_11 + Q_gyro * dt;
y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return x_angle;
}
// ********************************************************************
// ******** Tastaturabfrage zur Veränderung der PUI-Parameter *********
// ********************************************************************
int Tastatureingabe()
{
if(!Serial.available()) return 0;
char param = Serial.read(); // get parameter byte
if(!Serial.available()) return 0;
char cmd = Serial.read(); // get command byte
Serial.flush();
switch (param)
{
case 'p':
if(cmd=='+') Kp++;
if(cmd=='-') Kp--;
break;
case 'i':
if(cmd=='+') Ki += 0.1;
if(cmd=='-') Ki -= 0.1;
break;
case 'd':
if(cmd=='+') Kd++;
if(cmd=='-') Kd--;
break;
case 'k':
if(cmd=='+') K += 0.2;
if(cmd=='-') K -= 0.2;
break;
case 'l':
if(cmd=='+') K_Motor_links += 0.1;
if(cmd=='-') K_Motor_links -= 0.1;
break;
case 'r':
if(cmd=='+') K_Motor_rechts += 0.1;
if(cmd=='-') K_Motor_rechts -= 0.1;
break;
default:
Serial.print("?"); Serial.print(param);
Serial.print(" ?"); Serial.println(cmd);
}
Serial.println();
Serial.print("K:"); Serial.print(K);
Serial.print(" Kp:"); Serial.print(Kp);
Serial.print(" Ki:"); Serial.print(Ki);
Serial.print(" Kd:"); Serial.print(Kd);
Serial.print(" K_Motor_links:"); Serial.print(K_Motor_links);
Serial.print(" K_Motor_rechts:"); Serial.println(K_Motor_rechts);
}
No comments:
Post a Comment