succes
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 13 2022 09:29:34 by 1.7.2