philip/balancing-robot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Balancing success! With acceptable tune

Browse Source 
Still playing with acceleration limits and constrains on outputs and buffers in the controllers but robot works quite good now.
This commit is contained in:
philsson 2018-09-23 19:07:36 +02:00
parent 35479c6f0a
commit 20684701f5
2 changed files with 145 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

227
main.cpp
View File

@ -18,6 +18,7 @@
#include "src/control/lpf.h"
#include "src/control/PID.h"
#include "src/control/ImuFusion.h"
#include "src/control/MadgwickAHRS.h"
// Serialization
#include "src/serialization/RCProtocol.h"
@ -32,7 +33,12 @@ using namespace serialization;
EventQueue queue;
// Serial port (Servo Outputs)
Serial serial(PA_2, PA_3, 57600);
RawSerial serial(PA_2, PA_3, 250000);
RCProtocol RC;
// Pool to send Remote Control packages between threads
MemoryPool<RCProtocol::Packet, 16> mpool;
Queue<RCProtocol::Packet, 16> RCQueue;
// MPU setup
SPI spi(PA_7, PA_6, PA_5); //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
@ -41,135 +47,179 @@ mpu6000_spi imu(spi,PA_4); //define the mpu6000 object
PwmOut ledBlue(D4);
DigitalOut ledOrg(D5);
// Stepper motorL(PC_9, PB_1, PC_8);
Stepper motorL(PC_9, PC_7, PC_8);
Stepper motorR(PB_15, PB_14, PC_6);
Servo servo(PA_0);
// Interrupt pin from Gyro to MCU
InterruptIn gyroINT(PC_4);
InterruptIn gyroISR(PC_4);
// Madwick filter
Madgwick madgwick;
// Timer to calculate dT
Timer timer;
// TODO: Figure out some good values
controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40
// A PI controller for throttle control
controllerPI throttleControl(35.0, 3.0, 20, 60); // 35.0, 3.0, 20, 60
// TODO: Figure out some good values
controllerPD angleControl(10.0, 146.0, 400);
// Alternatively try controllerPD2 which has another Dterm implementation
// A PID for angle control
controllerPID angleControl(100.0f, 200.0f, 0.5f, 4000, 2000); // 100.0f, 200.0f, 0.5f, 4000, 2000
// Draft of some control function
// this runs in the context of eventThread and is triggered by the gyro ISR pin
void controlFunc()
// The control function to balance the robot
// Ran at high prio and triggered as an event by the gyro Interrupt pin
void runControl()
{
static ImuFusion imuFusion(&imu);
static float controlOutput(0.0f);
static RCProtocol::Packet remote;
osEvent evt = RCQueue.get(0);
if (evt.status == osEventMessage) {
RCProtocol::Packet* pPacket = (RCProtocol::Packet*)evt.value.p;
remote = *pPacket;
mpool.free(pPacket);
throttleControl.setGainScaling(remote.Ki, remote.Kd);
//throttleControl.setGainScaling(0, 0);
//angleControl.setGainScaling(remote.Kp, remote.Ki, remote.Kd);
angleControl.setGainScaling(remote.Kp, 0, -900);
servo.setPosition((float)remote.Throttle/1000.0f);
}
// Calculate dT in sec
float dT = timer.read_us()/(float)1000000.0;
float dT = timer.read_us()/(float)10e6;
timer.reset();
// Retrieve IMU angle (Only x-axis implemented for now)
float angleX = imuFusion.getAngle(dT);
float forceX = sin(angleX/180*PI);
// Retrieve IMU angle
madgwick.updateIMU(imu.read_rot(0),
imu.read_rot(1),
imu.read_rot(2),
imu.read_acc(0),
imu.read_acc(1),
imu.read_acc(2));
float angleX = madgwick.getPitch();
// Reset anything left in the IMU FIFO queue
imu.fifo_reset();
// If the robot is above this angle we turn off motors
static bool disabled = false;
if (abs(angleX) > (float)50.0f && !disabled)
if (!remote.Enabled || (abs(angleX) > (float)50.0f && !disabled))
{
controlOutput = 0.0f; // rinse integral
disabled = true;
motorL.disable();
motorR.disable();
}
else if (abs(angleX) < (float)50.0f && disabled)
else if (remote.Enabled && abs(angleX) < (float)10.0f && disabled)
{
disabled = false;
motorL.enable();
motorR.enable();
}
if (disabled)
{
throttleControl.flushIntegral();
angleControl.flushIntegral();
}
/* --------------------------
Calculate estimated groundspeed
of the robot.
TODO: Inject correct input (compensate for robot rotation)
of the robot in m/s
-------------------------- */
// Calculate filtered groundspeed of the robot (From deg/s to m/s)
float estimatedSpeed((motorL.getSpeed() + motorR.getSpeed())/2);
static incrementalLPF speedFilter;
float filteredEstSpeed(speedFilter.filter(estimatedSpeed));
float speedScale((1.0f/360.0f)*WHEEL_SIZE*PI);
float groundSpeed(filteredEstSpeed*speedScale);
// Static variables
static float angleXOld = 0;
static float motorAngularVelocity = 0;
static float motorAngularVelocityOld = 0;
static incrementalLPF filteredGroundSpeed;
if (disabled)
{
angleXOld = 0;
motorAngularVelocity = 0;
motorAngularVelocityOld = 0;
filteredGroundSpeed.clear();
}
// angular velocity - This way we get use of the madgwick filter
float angularVelocity = (angleX - angleXOld) * dT;
angleXOld = angleX;
// Motor Speed - Convert from deg/s to m/s
motorAngularVelocityOld = motorAngularVelocity;
motorAngularVelocity = ((motorL.getSpeed() + motorR.getSpeed())/2);
float estimatedGroundSpeed =
(motorAngularVelocityOld - angularVelocity)/360.0f*PI*WHEEL_SIZE;
estimatedGroundSpeed = filteredGroundSpeed.filter(estimatedGroundSpeed);
/* --------------------------
Calculate the setpoint for
the main control (control the angle)
through this throttle control loop (PI)
TODO: TEMP turned off the throttle
control to tune the PD angle control
-------------------------- */
float throttle(0.0f); // TODO: This will be the input from remote
float angleSP = 0.0f; // Temporarily emulate an output
//float angleSP = throttleControl.run(dT, groundSpeed, throttle);
float throttle = remote.Throttle*0.001f;
if (remote.Throttle < 50 && remote.Throttle > -50) {throttle = 0;}
// TODO: Figure out why we have the wrong sign here. I also had to reverse motors
// For the angle control loop. Is the madgwick filter outputting the wrong sign?
float angleSP = -throttleControl.run(dT, estimatedGroundSpeed, throttle);
/* --------------------------
The last control loop. Angle (PD)
TODO:
-------------------------- */
// Integrating the output to obtain acceleration
if (!disabled)
{
controlOutput += angleControl.run(dT, forceX, angleSP);
controlOutput = angleControl.run(dT, angleX, angleSP);
controlOutput = constrain(controlOutput, 3000.0f);
}
/* --------------------------
Lastly the steering is added
straight on the output
TODO: Activate when implemented
-------------------------- */
float steering(0.0f); // This will come from remote
float steering = remote.Steering*0.5;
if (remote.Steering < 50 && remote.Steering > -50) {steering = 0;}
motorL.setSpeed(controlOutput - steering);
motorR.setSpeed(controlOutput + steering);
// Blink LED at 1hz
static int i = 0;
i++;
if (i > 100)
if (++i > (disabled ? 25 : 100))
{
i = 0;
ledOrg = !ledOrg;
}
}
void serialContext()
void serialISR()
{
static RCProtocol RC;
while (true)
// Receive
while (serial.readable())
{
// Receive
while (serial.readable())
bool newPackage = RC.appendByte(serial.getc());
if (newPackage)
{
bool newPackage = RC.appendByte(serial.getc());
if (newPackage)
{
RCProtocol::Packet packet = RC.read();
}
RCProtocol::Packet* pPacket = mpool.alloc();
*pPacket = RC.read();
RCQueue.put(pPacket);
}
// Sleep some?
// Transmit
// serial.putc()...
}
// Sleep some?
//Thread::wait(1);
// Transmit
// serial.putc()...
}
// This context just pulses the blue LED
void pulseLedContext()
{
@ -192,13 +242,9 @@ void pulseLedContext()
// main() runs in its own thread
int main() {
// Serial in / out
Thread readUartThread;
readUartThread.start(callback(&serialContext));
// MPU startup at 100hz
if(imu.init(10,BITS_DLPF_CFG_188HZ)){
printf("\nCouldn't initialize MPU6000 via SPI!");
//printf("\nCouldn't initialize MPU6000 via SPI!");
}
wait(0.1);
@ -213,39 +259,54 @@ int main() {
imu.calib_acc(0);
calibrationResult = imu.resetOffset_acc();
// Setup Madwick filter
madgwick.begin(100);
/*-------------- Visible start sequence ------------*/
// Start sweeping the arm
//servo.sweep(0.0, 1, 2);
// Enable motor controllers (Will power the motors with no movement)
// motorL.enable();
// motorR.enable();
motorL.disable();
motorR.disable();
motorL.setDirection(-1);
motorR.setDirection(1);
// Servo Nod to tell us that we are done
//servo.nod();
//servo.setPosition(-0.2);
/*********************** Start all threads last **********************/
// Start the pulsing blue led
ledBlue.period_ms(10); // Any period. This will be overwritten by motors
Thread ledPulseThread;
//ledPulseThread.start(callback(&pulseLedContext));
Thread::wait(100);
// Serial in / out
serial.set_dma_usage_rx(DMA_USAGE_ALWAYS);
serial.attach(&serialISR);
Thread::wait(100);
// ISR won't be called if the serial is not emptied first.
serialISR();
// Enable/Activate the Gyro interrupt
imu.enableInterrupt();
// Start the timer used by the control loop
timer.start(); // Used to calc dT
/*-------------- Visible start sequence ------------*/
// Start sweeping the arm
servo.sweep(0.0, 1, 2);
// Start the pulsing blue led
Thread ledPulseThread;
ledPulseThread.start(callback(&pulseLedContext));
// Create realtime eventhandler for control loop
Thread eventThread(osPriorityRealtime);
Thread eventThread(osPriorityHigh);
eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
// Attach gyro interrupt to add a control event
gyroINT.rise(queue.event(&controlFunc));
// Enable motor controllers (Will power the motors with no movement)
motorL.enable();
motorR.enable();
motorL.setDirection(1);
motorR.setDirection(-1);
Thread::wait(1000);
// Servo Nod to tell us that we are done
servo.nod();
servo.setPosition(-0.2);
gyroISR.rise(queue.event(&runControl));
wait(osWaitForever);
}

2
src/drivers/stepper.cpp
View File

@ -12,7 +12,7 @@ Stepper::Stepper(PinName stepPin,
, m_dir(dirPin)
, m_en(enPin)
, m_accelerationLimitOn(true)
, m_accelerationLimit(50.0f)
, m_accelerationLimit(80.0f)
, m_stepsPerRevolution(200)
, m_microStepResolution(8)
, m_currentPeriod(0)