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

Deployment of control loops

Browse Source 
Find the bug! Needs fixing. Not tuned! And something crashes the board.
When vibrating a lot (probably high gains or high d-term / d-term noise) or when the robot leans a lot to one side everything freezes. Both loops blinking leds stop completely and steppers stay at last command.

I've tried electronic fixes and lastly put the controller on its own power supply.
Can be triggered even without any power electronics connected (rocking the robot by hand). So it seems to be software related!
This commit is contained in:
philsson 2018-09-02 20:21:58 +02:00 committed by Philip Johansson
parent 07b8299e25
commit 2d8f533a3e
3 changed files with 146 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
.vscode/settings.json vendored
View File

@ -4,6 +4,18 @@
"C_Cpp.intelliSenseEngine": "Tag Parser",
"files.associations": {
"cmath": "cpp",
"queue": "cpp"
"queue": "cpp",
"__bit_reference": "cpp",
"__functional_base": "cpp",
"atomic": "cpp",
"deque": "cpp",
"functional": "cpp",
"iterator": "cpp",
"limits": "cpp",
"memory": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"vector": "cpp",
"string": "cpp"
}
}

176
main.cpp
View File

@ -1,21 +1,30 @@
#include "mbed.h"
// STD
#include <cmath>
// Mbed
#include "mbed_events.h"
// target definitions
#include "src/targets/revo_f4/pins.h"
// Mmath
#include "src/math/Utilities.h"
// Drivers
#include "src/drivers/MPU6000.h"
#include "src/drivers/stepper.h"
#include "src/drivers/servo.h"
// Control
#include "src/control/lpf.h"
#include "src/control/PID.h"
#include "src/control/ImuFusion.h"
#ifndef PI
#define PI 3.14159265358979f
#endif
#define WHEEL_SIZE 0.09f
using namespace targets::revo_f4;
using namespace drivers;
using namespace control;
using namespace math;
EventQueue queue;
@ -24,21 +33,100 @@ EventQueue queue;
SPI spi(PA_7, PA_6, PA_5); //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
mpu6000_spi imu(spi,PA_4); //define the mpu6000 object
PwmOut led1(D4); //PB_5
PwmOut ledBlue(D4);
DigitalOut ledOrg(D5);
Stepper motor1(PC_9, PC_7, PC_8);
Stepper motor2(PB_15, PB_14, PC_6);
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);
Timer timer;
// TODO: Figure out some good values
controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40
// TODO: Figure out some good values
controllerPD angleControl(25.0, 35.25, 400);
// Alternatively try controllerPD2 which has another Dterm implementation
// Draft of some control function
// this runs in the context of eventThread and is triggered by the gyro ISR pin
void controlFunc()
{
static ImuFusion imuFusion(&imu);
static float controlOutput(0.0f);
// Calculate dT in sec
float dT = timer.read_us()/(float)1000000.0;
timer.reset();
// Retrieve IMU angle (Only x-axis implemented for now)
float angleX = imuFusion.getAngle(dT);
// 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)
{
controlOutput = 0.0f; // rinse integral
disabled = true;
}
else if (abs(angleX) < (float)50.0f && disabled)
{
disabled = false;
}
/* --------------------------
Calculate estimated groundspeed
of the robot.
TODO: Inject correct input (compensate for robot rotation)
-------------------------- */
// 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);
/* --------------------------
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);
/* --------------------------
The last control loop. Angle (PD)
TODO:
-------------------------- */
// Integrating the output to obtain acceleration
if (!disabled)
{
controlOutput += angleControl.run(dT, angleX, angleSP);
controlOutput = constrain(controlOutput, 800.0f);
}
/* --------------------------
Lastly the steering is added
straight on the output
TODO: Activate when implemented
-------------------------- */
float steering(0.0f); // This will come from remote
motorL.setSpeed(controlOutput - steering);
motorR.setSpeed(controlOutput + steering);
// Blink LED at 1hz
static int i = 0;
i++;
if (i > 100)
@ -46,12 +134,6 @@ void controlFunc()
i = 0;
ledOrg = !ledOrg;
}
static incrementalLPF filter;
double accY = 1;
accY = imu.read_acc(1);
double filtered = filter.filter(accY);
motor1.setSpeed(360.0*2*filtered);
}
// This context just pulses the blue LED
@ -67,7 +149,7 @@ void pulseLedContext()
// We cannot set pulsewidth or period due to
// sharing timer with motor outputs. "write"
// instead sets the dutycycle in range 0-1
led1.write(out);
ledBlue.write(out);
Thread::wait(10);
}
}
@ -78,49 +160,55 @@ int i = 0;
// main() runs in its own thread
int main() {
// Start sweeping the arm
servo.sweep(0.0, 1, 2);
Thread ledPulseThread;
ledPulseThread.start(callback(&pulseLedContext));
//
Thread eventThread;
eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
// Function to attach to gyro interrupt
gyroINT.rise(queue.event(&controlFunc));
// Enable motor controllers (Will power the motors with no movement)
motor1.enable();
motor2.enable();
// Set motor 2 to a fixed speed
// (To test that timers don't interfeer with each other)
motor2.setSpeed(90.0);
// MPU startup
// MPU startup at 100hz
if(imu.init(10,BITS_DLPF_CFG_188HZ)){
printf("\nCouldn't initialize MPU6000 via SPI!");
}
wait(0.1);
// Set IMU scale
int accScale = imu.set_acc_scale(BITS_FS_16G);
wait(0.1);
int gyroScale = imu.set_gyro_scale(BITS_FS_2000DPS);
wait(0.1);
// Calibrate and Trim acc & gyro
bool calibrationResult = imu.resetOffset_gyro();
imu.calib_acc(0);
calibrationResult = imu.resetOffset_acc();
// Enable/Activate the Gyro interrupt
imu.enableInterrupt();
// Read some test values
int wmi = imu.whoami();
int scale = imu.set_acc_scale(BITS_FS_16G);
// 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);
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();
Thread::wait(1000);
servo.sweep(1.0, 1.5, 3);
servo.setPosition(-0.2);
wait(osWaitForever);

1
src/control/ImuFusion.cpp
View File

@ -8,6 +8,7 @@ ImuFusion::ImuFusion(mpu6000_spi* pImu)
{
}
// TODO: Implement for all axis
float ImuFusion::getAngle(float dT)
{
int axis = 0;