main.cpp

00001 #include "MicroBit.h"
00002 #include "sample.h"
00003 #include "motor.h"
00004 #include "PID.h"
00005 #include "kalman.h"
00006 #include "mbed.h"
00007 #include "BNO055.h"
00008 
00009 MicroBit uBit;
00010 
00011 Ticker stepInterrupt;
00012 
00013 int main()
00014 {
00015     
00016     BNO055::Sensor sensor(uBit);
00017     
00018     sensor.Calibrate();
00019  
00020    // Initialize PID controller
00021     PID pid;
00022     pid_zeroize(&pid);
00023     pid.windup_guard = 0; 
00024     pid.proportional_gain = 95;
00025     pid.integral_gain = 0.75;
00026     pid.derivative_gain = 0.5;
00027   
00028    // Initialize motor and set up step interrupt
00029     Motor motor(uBit);
00030     stepInterrupt.attach(&motor, &Motor::Step, STEP_PERIOD);  
00031 
00032     char logMessage[200];
00033     float error = 0;
00034     float dt;
00035     float loopTime = 0.003; 
00036     double heading = 0;
00037     double roll = 0;
00038     double pitch = 0;
00039     while(true)
00040     {
00041        Timer t;
00042         t.start();
00043       
00044         sensor.ReadEulerAngles(heading, roll, pitch);
00045         
00046         error = 89-roll;
00047         
00048         pid_update(&pid, error, dt); 
00049   
00050         if (error < 0)
00051             motor.SetDirection(FORWARD);
00052         else
00053             motor.SetDirection(REVERSE);
00054      
00055   
00056         motor.SetSpeed(pid.control);      
00057        
00058         t.stop();
00059         dt = t.read();
00060         if (dt < loopTime)
00061             wait(loopTime-dt);
00062    
00063         //sprintf(logMessage, "h: %f, r: %f, p: %f\n", heading, roll, pitch);
00064         //uBit.serial.send(logMessage);
00065  
00066     }
00067         
00068     release_fiber();
00069 }
00070 
00071 
00072