succes
main.cpp
- Committer:
- mk1
- Date:
- 2017-02-11
- Revision:
- 0:c15430f1895f
File content as of revision 0:c15430f1895f:
#include "MicroBit.h" #include "sample.h" #include "motor.h" #include "PID.h" #include "kalman.h" #include "mbed.h" #include "BNO055.h" MicroBit uBit; Ticker stepInterrupt; int main() { BNO055::Sensor sensor(uBit); sensor.Calibrate(); // Initialize PID controller PID pid; pid_zeroize(&pid); pid.windup_guard = 0; pid.proportional_gain = 95; pid.integral_gain = 0.75; pid.derivative_gain = 0.5; // Initialize motor and set up step interrupt Motor motor(uBit); stepInterrupt.attach(&motor, &Motor::Step, STEP_PERIOD); char logMessage[200]; float error = 0; float dt; float loopTime = 0.003; double heading = 0; double roll = 0; double pitch = 0; while(true) { Timer t; t.start(); sensor.ReadEulerAngles(heading, roll, pitch); error = 89-roll; pid_update(&pid, error, dt); if (error < 0) motor.SetDirection(FORWARD); else motor.SetDirection(REVERSE); motor.SetSpeed(pid.control); t.stop(); dt = t.read(); if (dt < loopTime) wait(loopTime-dt); //sprintf(logMessage, "h: %f, r: %f, p: %f\n", heading, roll, pitch); //uBit.serial.send(logMessage); } release_fiber(); }