Hardware components | ||||||
 |
| × | 1 | |||
 |
| × | 1 | |||
| × | 2 | ||||
 |
| × | 1 | |||
 |
| × | 1 | |||
 |
| × | 2 | |||
| × | 1 | ||||
| × | 1 | ||||
_wzec989qrF.jpg?auto=compress%2Cformat&w=48&h=48&fit=fill&bg=ffffff) |
| × | 1 | |||
New: control of the robot via Joystick over Bluetooth.
See redesign of the Robot:
I bought my first Arduino three years ago. I was fascinated by by the idea of a self-balancing-robot and this was my first project.
The two wheeled self balancing robot represents a robotic platform with two independently actuated wheels and center of gravity above the axis of the wheels rotation.The behavior of the robot is similar to the classical mechanical system of an inverted pendulum.
I underestimated the difficulties, so the development took a long time. Many changes were necessary:
The most important changes were:
- Change from Arduino Mega to Arduino DUE
- Change from direct-current-motor with encoder to stepper motor
- Use of digital motion processing
- MP 6500 stepper motor driver carrier
- Control of the robot via Joy Stick using Bluetooth
- Stepper Motor, Unipolar/Bipolar, 200 Steps/Rev, 42×48mm, 4V, 1.2 A/Phase
- Stepper Motor Driver Carrier can deliver up to 1.5 A per phase continuously, four different step resolutions: full-step, half-step, 1/4-step, and 1/8-step.
- cascaded PID Controller for Motor and for Position
- Task Dispatcher via Interrupt
- PWM Controller
- MPU-6050 sensor with accelerometer and gyro, using Digital Motion Processing with MPU-6050
- Auto Tuning via Twiddle Algorithmus
- Battery Control
- Software Design Object oriented
I decided to use Stepper engines because they offer the following advantages:
- Exact positioning, no accumulated errors
- Holding torque in rest position
- No deceleration/lag due to the moment of inertia of the motor
- simple position sensing by counting PWM signal
The MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single chip. It is very accurate, as it contains 16-bits analog to digital conversion hardware for each channel. Therefor it captures the x, y, and z channel at the same time. The sensor uses the I2C -bus to interface with the Arduino.
I used Digital Motion Processing with the MPU-6050 sensor, to do fast calculations directly on the chip. This reduces the load for the Arduino.
https://playground.arduino.cc/Main/MPU-6050
Because of the orientation of my board, I used yaw/pitch/roll angles (in degrees) calculated from the quaternions coming from the FIFO. By reading Euler angle I got problems with Gimbal lock.
MP6500 Stepper Motor Driver CarrierThe used stepper motor driver lets you control one bipolar stepper motor at up to approximately 1.5 A per phase continuously without a heat sink or forced air flow ( Datasheet).
PID auto Tuning with TwiddleTwiddle is an algorithm that tries to find a good choice of parameters for an algorithm. Also known as Hill climbing, it is an analogy to a mountaineer who looks for the summit in dense fog and steers his steps as steeply uphill as possible. If it only goes down in all directions, he has arrived at a summit.The Twiddle algorithm is used for auto tuning of PID parameter. First of all, parameters can be tested with a manual tuning with a potentiometer.
Cascaded PID ControlerThe robot is controlled by cascaded PID controllers. Two controllers are responsible for driving the motors (right and left side). Two additional controllers for controlling the position of the robot.
The motor controller ensures that the robot remains upright. The Position Controler ensures that the robot maintains its correct setpoint prescribed position.
Cascade control is a cascading of several controllers; the associated control loops are nested in each other. The controller output variable of one controller (master controller, Position) serves as the reference variable for another (slave controller, Motor).
The PID controllers are implemented in the "PIDControl" class. 4 instances of the class are instantiated for the 4 controllers.
The standard PID algorithm was slightly modified after longer test series. To the P-component the parameter Kx multiplied by the I-component was added.
Before this change the robot always wanted to run away. A simple increase of the I-portion made the robot unstable. This solution certainly depends strongly on the structure of the robot (weight, centre of gravity, etc.).
PWM ControlerTo generate the PWM signals I modified the library of "randomvibe . (https://github.com/cloud-rocket/DuePWM). The PWM controller is implemented in the DuePWMmod class. Unique frequencies set via PWM Clock-A ("CLKA") and Clock-B ("CLKB").
Task DispatchingThe Dispatching of the tasks:
- Robot
- LCD
- plotter
is done with the help of the Timer Library by Ivan Seidel.( https://github.com/ivanseidel/DueTimer thanks)Three interrupts with the corresponding times are generated to call the tasks.
Software architectureThe robot consists of the main program SBRobotDue02 and the following classes:
- PidControl
- Battery
- DuePWMmod
- DueTimer
- Twiddle
- Motor
- Vehicle
- MyMPU
and the following includes:
- Config
- LCD
- PidParameter
- Plotter
The architecture is shown in the following UML Diagrams:
The folowing sequence diagram shows the interactions between instanced objects in the sequential order that those interactions occur.
The run method of the Vehicle class is crucial for control the Robotin all directions.
/**********************************************************************/
void Vehicle::Run(float angle, int iPositionA, int iPositionB, JStickData JStick)
/**********************************************************************/
{ /*
Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev => 1.8 degree per step
Wheel Diameter 88mm = > Distance per Pulse Dpp = d * pi / 200 = 1,3816 mm
Distance per Revolution = 276,32 mm
Max 1000 Steps per Second = 5 Revolutions => 13816 mm Distance
*/
const int tDelay = 10;
const float MaxSpeed = 1.6; //1.5; 11.03.19
static bool decelerate = false;
/* "iPositionA" in microsteps
8 microsteps = 1 full Step
1 microstepp = 0,125 full steps
after division one change in "PositionA" = 0.01 microstepp and 0,0125 full steps = 0,01727 mm
*/
PositionA = (float(iPositionA ) / 100 );
PositionB = (float(iPositionB ) / 100 );
if (firstRun) {
firstRun = false;
skipPos = - (2 * tDelay);
}
if (abs(JStick.Xvalue) > 5) spinning = true; else spinning = false;
DeltaPosA = (float(JStick.Yvalue) / 15.0) - (float(JStick.Xvalue) / 250.0) ;
DeltaPosB = (float(JStick.Yvalue) / 15.0) + (float(JStick.Xvalue) / 250.0);
if (++skipPos >= tDelay) { // to delay the calls, Position Control should be 10 times lower than Motor Control
skipPos = 0;
SetpointPositionA = PositionA + DeltaPosA;
SetpointPositionB = PositionB + DeltaPosB;
SetpointA = constrain( (pPidDistA->calculate (PositionA , SetpointPositionA)), (SetpointAold - MaxSpeed), (SetpointAold + MaxSpeed));
SetpointB = constrain( (pPidDistB->calculate (PositionB , SetpointPositionB)), (SetpointBold - MaxSpeed), (SetpointBold + MaxSpeed));
SetpointA = ( SetpointA * 0.7) + (SetpointAold * 0.3); //low-pass filter
SetpointB = ( SetpointB * 0.7) + (SetpointBold * 0.3);
SetpointAold = SetpointA ;
SetpointBold = SetpointB ;
}
// PID calculation of new steps
StepsA = pMotorA->Calculate(angle, -SetpointA);
StepsB = pMotorB->Calculate(angle, -SetpointB);
//After the robot has moved to the right or left, slight deviations occur during the next straight ride.
//workaound: when driving straight forwad or backwad(!spinning), the equality of the steps is forced.
if (!spinning) {
ABdiff = StepsA - StepsB;
}
// run the Motors, here Steps = full steps
pMotorA->Run(StepsA );
pMotorB->Run(StepsB + ABdiff);
}
/**********************************************************************/
void Vehicle::Stop() {
pMotorA->SleepMode( );
pMotorB->SleepMode( );
}
The following query ensures that position control is called 10 times slower than motor control.
if (++skipPos >= tDelay) { // to delay the calls, Position Control should be 10 times lower than Motor Control
skipPos = 0;
last baut not least,the PID control for the Motor (Steps /sec) is needed for the robot (inner PID controler).
// PID calculation of new steps
StepsA = pMotorA->Calculate(angle, -SetpointA);
StepsB = pMotorB->Calculate(angle, -SetpointB);
After the robot has moved to the right or left, slight deviations occur during the next straight ride.
Workaound: when driving straight forwad or backwad(!spinning), the equality of the steps is forced.
if (!spinning) {
ABdiff = StepsA - StepsB;
}
// run the Motors, here Steps = full steps
pMotorA->Run(StepsA );
pMotorB->Run(StepsB + ABdiff);
}
Thats all to drive the robot.
PID controler in detailIn the following diadram you can see the reaction of the cascaded PID controller after a disturbance.
The standard PID algorithm was slightly modified after longer test series.
There are 4 instances of the class PidControl. Two instances for motor control and two instances for position control. The calculation is done in the method PidControl::calculate.
/**********************************************************************/
float PidControl::calculate (float iAngle, float isetPoint )
/**********************************************************************/
// new calculation of Steps per Second // PID algorithm
{
Now = micros();
if (first) {
first = false;
Last_time = Now;
integrated_error = 0;
}
timeChange = (Now - Last_time) ;
timeChange = timeChange / 1000.0; // in millisekunden
error = isetPoint - iAngle;
if ( timeChange != 0) {
dTerm = 1000.0 * Kd * (error - Last_error) / timeChange ;
}
integrated_error = integrated_error + ( error * timeChange );
iTerm = Ki * integrated_error / 1000.0;
pTerm = Kp * error + ( Ka * integrated_error ); // modifying Kp
// Compute PID Output in Steps per second
eSpeed = K * (pTerm + iTerm + dTerm) ;
/*Remember something*/
Last_time = Now;
Last_error = error;
if (eSpeed > 0 ) {
DirForward = true ;
} else {
DirForward = false;
}
return eSpeed; // Steps per Second
}
The instances of the classes are created in the sketch SBRobotDue02.
// ----------------------------------------------------------------------
// create PID Controller for Motor A and B
// -----------------------------------------------------------------------
#include "PidControl.h"
PidParameter PidParams;
PidControl PidMotorA(PidParams);
PidControl PidMotorB(PidParams);
// -----------------------------------------------------------------------
// create PID Controller Position A and B
// -----------------------------------------------------------------------
PidParameterDi PidParamsDi;
PidControl PidDistA(PidParamsDi);
PidControl PidDistB(PidParamsDi);
The different PID parameters are defined in the following structures and are passed during instanciation of the class. (C++ Overloading of the constructor )
// PidParameter Motor
struct PidParameter {
float K = 5.0;
float Kp = 9.3 ;
float Ki = 6.01;
float Kd = 0.12 ;
float Ka = 0.1246;
float Kx = 0.0; //
};
// PidParameter Position
struct PidParameterDi {
float K = 1.0;
float Kp = 0.18;
float Ki = 0.0;
float Kd = 0.4 ;
float Ka = 0.0 ;
float Kx = 0.0;
};
To drive the motors, two instances of the motor class are created.
// -----------------------------------------------------------------------
// create objects for Motor 1 and Motor 2
// -----------------------------------------------------------------------
#include "Motor.h"
Motor MotorA(&pwm, &PidMotorA, PinDirMotorA, PinStepMotorA, PinMs1MotorA,
PinMs2MotorA, PinSleepA, rechterMotor); // create MotorA
Motor MotorB(&pwm, &PidMotorB, PinDirMotorB, PinStepMotorB, PinMs1MotorB,
PinMs2MotorB, PinSleepB, linkerMotor); // create MotorB
The run method is responsible for controlling the stepper motor.
/**********************************************************************/
void Motor::Run(float Steps) {
/**********************************************************************/
if (Steps >= 0 ) {
if (_MotorSide == rechterMotor) {
digitalWrite(_PinDir, LOW);
}
else {
digitalWrite(_PinDir, HIGH);
}
} else
{
if (_MotorSide == rechterMotor) {
digitalWrite(_PinDir, HIGH);
}
else {
digitalWrite(_PinDir, LOW);
}
}
if (_Divisor > 0) {
Steps = Steps * _Divisor; // Microsteps
}
Steps_tmp = Steps;
Steps = abs(Steps_tmp);
if ( Steps < 2.0) {
Steps = 2; // Microsteps
}
if (_MotorSide == rechterMotor) {
ptrpwm->setFreq( Steps, rechterMotor );
}
else {
ptrpwm->setFreq( Steps, linkerMotor );
}
}
The method setFreq of the class DuePWMmod is called to generate the pwm signals.
Use of interruptsThe robot usesdifferent interrupts.
MPU 6050 interrupt number 2. The FIFO of the MPU 6050 buffer is used together with the interrupt 2. If the MPU-6050 places data in the FIFObuffer, it signals the Arduino with the interrupt signal, so the Arduinoknows that there is data in the FIFO buffer waiting to be read.
Interrupts 4, 5 and6 are used to control the following tasks:
- Plotter
- LCD Anzeige
- Run Mode
The Timer library byIvan Seidel is used for this purpose.
Timer4.attachInterrupt(PlotterISR).setPeriod(8000).start();// Plotter 80 ,secc
Timer3.attachInterrupt(LcdTimer).setPeriod(500000).start(); // LCDDisplay 500 msec
Timer6.attachInterrupt(RobotFlag).setPeriod(LoopTimeMicrosec).start();// RobotFlag 12 msec
/**********************************************************************/
// Timer 6 Robot ISR Routine
/**********************************************************************/
void RobotFlag() {
RunFlag = true;
}
// --------------------------------------------------------------
// Run Robot
// --------------------------------------------------------------
if ( RunFlag ) {
RunFlag = false;
The current position of the stepper motors is measured via the Step pins by interrupt.
attachInterrupt(digitalPinToInterrupt(PinStepMotorA), ISR_PWMA, RISING);
attachInterrupt(digitalPinToInterrupt(PinStepMotorB), ISR_PWMB, RISING);
void ISR_PWMA() {
if (PidMotorA.DirForward ) {
PositionA ++;
} else {
PositionA --;
}
}
The serialEvent1 interrupt is used to receive data from the joy stick. SerialEvent occurs whenever a new data comes in the hardware serial RX.
void serialEvent1() {
while (Serial1.available()) {
// get the new byte:
char inChar = (char)Serial1.read();
// add it to the inputString:
if (!stringComplete) {
inputString += inChar;
}
// if the incoming character is a newline, set a flag so the main loop
// can do something about it:
if (inChar == '\n') {
stringComplete = true;
}
}
}
The Funduino Joystick Shield V1.A is used to control the robot in all directions. The shield is using the Arduino Mega. The data from the shield are received via a serial event over a Bluetooth connection. The sketch JoyStickSlave01 sends data to the robot as soon as something has changed.
if ((Xvalue == map(analogRead(JOYSTICK_AXIS_X), 0, 1023, -100, 100)) &&
(Yvalue == map(analogRead(JOYSTICK_AXIS_Y), 0, 1023, -100, 100)) &&
(JButton == digitalRead(JOYSTICK_BTN)) &&
(Up == digitalRead(UP_BTN)) &&
(Down == digitalRead(DOWN_BTN)) &&
(Left == digitalRead(LEFT_BTN)) &&
(Right == digitalRead(RIGHT_BTN))) return;
Xvalue = map(analogRead(JOYSTICK_AXIS_X), 0, 1023, -100, 100);
Yvalue = map(analogRead(JOYSTICK_AXIS_Y), 0, 1023, -100, 100);
JButton = digitalRead(JOYSTICK_BTN);
Up = digitalRead(UP_BTN);
Down = digitalRead(DOWN_BTN);
Left = digitalRead(LEFT_BTN);
Right = digitalRead(RIGHT_BTN);
The data of the Joy Stick are separated by the character "#". Before each date, an identifier is transferred (xml like).
Serial.print("X");
Serial.print("#");
Serial.print(Xvalue);
Serial.print("#");
Serial.print("Y");
Serial.print("#");
Serial.print(Yvalue);
Serial.print("#");
Serial.print("B1");
Serial.print("#");
Serial.print(JButton);
Serial.print("#");
Serial.print("Up");
Serial.print("#");
Serial.print(Up);
Serial.print("#");
Serial.print("Do");
Serial.print("#");
Serial.print(Down);
Serial.print("#");
Serial.print("Le");
Serial.print("#");
Serial.print(Left);
Serial.print("#");
Serial.print("Ri");
Serial.print("#");
Serial.print(Right);
Serial.print("#");
Serial.print('\n');
The Robot receives the Date and stor the Data in the following structure.
struct JStickData {
int Xvalue, Yvalue, Up, Down, Left, Right, JButton;
};
The data are parsed with the following method.
void BTRead( JStickData &JSData ) {
String command;
unsigned int j;
long value;
// print the string when a newline arrives:
if (stringComplete) {
if (inputString.substring(0, 1) != "X")
{
Serial.print("Error reading Bluetooth ");
Serial.println(inputString);
} else {
j = 0;
for (unsigned int i = 0; i < inputString.length(); i++) {
if (inputString.substring(i, i + 1) == "#") {
command = inputString.substring(j, i);
// Serial.print("Command: ");
// Serial.print(command);
j = i + 1;
for (unsigned int i1 = j; i1 < inputString.length(); i1++) {
if (inputString.substring(i1, i1 + 1) == "#") {
value = inputString.substring(j, i1).toInt();
// Serial.print(" Value: ");
// Serial.println(value);
i = i1;
j = i + 1;
assignValues(command, value, JSData);
break;
}
}
}
}
}
inputString = "";
stringComplete = false;
// Serial.print(" Value: ");
// Serial.println(JStick.Xvalue);
}
}
and assigned as follows.
void assignValues(String icommand, int ivalue, JStickData &Data ) {
if (icommand == "X") Data.Xvalue = ivalue;
if (icommand == "Y") Data.Yvalue = ivalue;
if (icommand == "B1") Data.JButton = ivalue;
if (icommand == "Up") Data.Up = ivalue;
if (icommand == "Do") Data.Down = ivalue;
if (icommand == "Ri") Data.Right = ivalue;
if (icommand == "Le") Data.Left = ivalue;
}
Currently only the data for x and y are required.
_3u05Tpwasz.png?auto=compress%2Cformat&w=40&h=40&fit=fillmax&bg=fff&dpr=2)
Comments