diff --git a/main.cpp b/main.cpp index de58ee9..df798bb 100644 --- a/main.cpp +++ b/main.cpp @@ -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 mpool; +Queue 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); } \ No newline at end of file diff --git a/src/drivers/stepper.cpp b/src/drivers/stepper.cpp index 9558797..3aa7996 100644 --- a/src/drivers/stepper.cpp +++ b/src/drivers/stepper.cpp @@ -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)