diff --git a/.vscode/settings.json b/.vscode/settings.json index 7b646a5..3a12360 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -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" } } \ No newline at end of file diff --git a/main.cpp b/main.cpp index e42b925..24b9178 100644 --- a/main.cpp +++ b/main.cpp @@ -1,21 +1,30 @@ #include "mbed.h" +// STD #include +// 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); diff --git a/src/control/ImuFusion.cpp b/src/control/ImuFusion.cpp index e7c9508..bf72c3c 100644 --- a/src/control/ImuFusion.cpp +++ b/src/control/ImuFusion.cpp @@ -8,6 +8,7 @@ ImuFusion::ImuFusion(mpu6000_spi* pImu) { } +// TODO: Implement for all axis float ImuFusion::getAngle(float dT) { int axis = 0;