Marek Korczak / nrf51_microbit

succes

Dependencies:   microbit

main.cpp@0:c15430f1895f, 2017-02-11 (annotated)

Committer:
mk1
Date:
Sat Feb 11 15:55:34 2017 +0000
Revision:
0:c15430f1895f
succes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mk1 0:c15430f1895f 1 #include "MicroBit.h"
mk1 0:c15430f1895f 2 #include "sample.h"
mk1 0:c15430f1895f 3 #include "motor.h"
mk1 0:c15430f1895f 4 #include "PID.h"
mk1 0:c15430f1895f 5 #include "kalman.h"
mk1 0:c15430f1895f 6 #include "mbed.h"
mk1 0:c15430f1895f 7 #include "BNO055.h"
mk1 0:c15430f1895f 8
mk1 0:c15430f1895f 9 MicroBit uBit;
mk1 0:c15430f1895f 10
mk1 0:c15430f1895f 11 Ticker stepInterrupt;
mk1 0:c15430f1895f 12
mk1 0:c15430f1895f 13 int main()
mk1 0:c15430f1895f 14 {
mk1 0:c15430f1895f 15
mk1 0:c15430f1895f 16 BNO055::Sensor sensor(uBit);
mk1 0:c15430f1895f 17
mk1 0:c15430f1895f 18 sensor.Calibrate();
mk1 0:c15430f1895f 19
mk1 0:c15430f1895f 20 // Initialize PID controller
mk1 0:c15430f1895f 21 PID pid;
mk1 0:c15430f1895f 22 pid_zeroize(&pid);
mk1 0:c15430f1895f 23 pid.windup_guard = 0;
mk1 0:c15430f1895f 24 pid.proportional_gain = 95;
mk1 0:c15430f1895f 25 pid.integral_gain = 0.75;
mk1 0:c15430f1895f 26 pid.derivative_gain = 0.5;
mk1 0:c15430f1895f 27
mk1 0:c15430f1895f 28 // Initialize motor and set up step interrupt
mk1 0:c15430f1895f 29 Motor motor(uBit);
mk1 0:c15430f1895f 30 stepInterrupt.attach(&motor, &Motor::Step, STEP_PERIOD);
mk1 0:c15430f1895f 31
mk1 0:c15430f1895f 32 char logMessage[200];
mk1 0:c15430f1895f 33 float error = 0;
mk1 0:c15430f1895f 34 float dt;
mk1 0:c15430f1895f 35 float loopTime = 0.003;
mk1 0:c15430f1895f 36 double heading = 0;
mk1 0:c15430f1895f 37 double roll = 0;
mk1 0:c15430f1895f 38 double pitch = 0;
mk1 0:c15430f1895f 39 while(true)
mk1 0:c15430f1895f 40 {
mk1 0:c15430f1895f 41 Timer t;
mk1 0:c15430f1895f 42 t.start();
mk1 0:c15430f1895f 43
mk1 0:c15430f1895f 44 sensor.ReadEulerAngles(heading, roll, pitch);
mk1 0:c15430f1895f 45
mk1 0:c15430f1895f 46 error = 89-roll;
mk1 0:c15430f1895f 47
mk1 0:c15430f1895f 48 pid_update(&pid, error, dt);
mk1 0:c15430f1895f 49
mk1 0:c15430f1895f 50 if (error < 0)
mk1 0:c15430f1895f 51 motor.SetDirection(FORWARD);
mk1 0:c15430f1895f 52 else
mk1 0:c15430f1895f 53 motor.SetDirection(REVERSE);
mk1 0:c15430f1895f 54
mk1 0:c15430f1895f 55
mk1 0:c15430f1895f 56 motor.SetSpeed(pid.control);
mk1 0:c15430f1895f 57
mk1 0:c15430f1895f 58 t.stop();
mk1 0:c15430f1895f 59 dt = t.read();
mk1 0:c15430f1895f 60 if (dt < loopTime)
mk1 0:c15430f1895f 61 wait(loopTime-dt);
mk1 0:c15430f1895f 62
mk1 0:c15430f1895f 63 //sprintf(logMessage, "h: %f, r: %f, p: %f\n", heading, roll, pitch);
mk1 0:c15430f1895f 64 //uBit.serial.send(logMessage);
mk1 0:c15430f1895f 65
mk1 0:c15430f1895f 66 }
mk1 0:c15430f1895f 67
mk1 0:c15430f1895f 68 release_fiber();
mk1 0:c15430f1895f 69 }
mk1 0:c15430f1895f 70
mk1 0:c15430f1895f 71
mk1 0:c15430f1895f 72