Diff: main.cpp

Revision:

0:7f6319a73219

Child:

1:0acb5ca94031

diff -r 000000000000 -r 7f6319a73219 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 28 23:16:23 2022 +0000
@@ -0,0 +1,169 @@
+// Calibrate and arm ESC and then sweep through motor speed range
+// To run: Hold down reset on mbed and power up ESC motor supply
+// Wait for three power up beeps from ESC, then release reset (or apply power) on mbed
+// See https://github.com/bitdump/BLHeli/blob/master/BLHeli_S%20SiLabs/BLHeli_S%20manual%20SiLabs%20Rev16.x.pdf
+// for info on beep codes and calibration
+#include "mbed.h"
+#include "Servo.h"
+#include "MPU6050.h"
+#include "ledControl.h"
+#include "PID.h"
+
+PwmOut ledf(LED1); //throttle up test led with PWM dimming
+PwmOut ledr(LED2); //throttle down test led with PWM dimming
+
+Servo ESC0(p21); //Back  Left
+Servo ESC1(p22); //Front Left
+Servo ESC2(p24); //Front Right
+Servo ESC3(p23); //Back  Right
+
+Serial pc(USBTX, USBRX); //Initalise PC serial comms
+Serial Receiver(p13, p14); //Initialize Receiver Serial comms
+MPU6050 mpu6050;           // class: MPU6050, object: mpu6050
+Ticker toggler1;
+Ticker filter;
+
+//using namespace std::chrono;
+
+void toggle_led1();
+void toggle_led2();
+void compFilter();
+
+float pitchAngle = 0;
+float rollAngle = 0;
+float yawAngle = 0;
+
+float pitchTarget = 0;
+float rollTarget = 0;
+float yawTarget = 0;
+
+PID PitchPID(0.5,1,0.01);
+PID RollPID(0.5,1,0.01);
+PID YawPID(0.5,1,0.01);
+
+float ThrustValue = 0;
+
+// Receiver variables
+uint16_t channel[14];
+
+Timer t;
+
+int main()
+{
+    ESC0 = ESC1 = ESC2 = ESC3 = 0;
+    ledf = ledr = 0;
+    ESC0 = ESC1 = ESC2 = ESC3 = 0;
+    wait(0.5);
+    ESC0 = ESC1 = ESC2 = ESC3 = 0.2;
+    wait(0.5);
+    ESC0 = ESC1 = ESC2 = ESC3 = 0;
+    wait(0.5);
+
+    uint8_t byte1, byte2;
+    //bool ready_to_arm = false;
+    pc.baud (115200);
+    Receiver.baud(115200);
+
+    mpu6050.whoAmI();                           // Communication test: WHO_AM_I register reading
+    wait(1);
+    mpu6050.calibrate(accelBias,gyroBias);      // Calibrate MPU6050 and load biases into bias registers
+    pc.printf("Calibration is completed. \r\n");
+    wait(0.5);
+    mpu6050.init();                             // Initialize the sensor
+    wait(1);
+    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
+    wait_ms(500);
+    filter.attach(&compFilter, 0.005);    // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
+    rollTarget = rollAngle;
+    pitchTarget = pitchAngle;
+    yawTarget = yawAnglw;
+
+    PitchPID.setLimit(-0.25,0.25);
+    RollPID.setLimit(-0.25,0.25);
+    YawPID.setLimit(-0.25,0.25);
+
+    pc.printf("Ready to go...\n\r");
+
+    float rollAdjustment = 0;
+    float pitchAdjustment = 0;
+    float yawAdjustment = 0;
+
+    while (1) {
+        t.start();
+
+        //PitchPID.setProcessValue(pitchAngle);
+        //pc.printf("| Pitch: %.5f | Output:%.5f |\r\n",pitchAngle,PitchPID.compute());
+
+        //PitchPID.setProcessValue(rollAngle);
+        //pc.printf("| Roll: %.5f | Output:%.5f |\r\n\n",rollAngle,PitchPID.compute());
+
+        //float imuRoll = 0;
+        //float imuPitch = 0;
+
+        if (Receiver.readable()) {
+            if(Receiver.getc() == 0x20) {
+                if(Receiver.getc() == 0x40) {
+                    //printf("Message Recieved! \n");
+                    for (int i = 0; i<3; i++) {
+                        byte1 = Receiver.getc();
+                        //byte2 = Receiver.getc();
+                        //if (i == 2)
+                        //    printf("Throttle: %02X %02X Sum: %d \n\r", (byte1&0x000000FF), byte2, channel[i]);
+                        channel[i] = (Receiver.getc() << 8);
+                        channel[i] += (byte1&0x000000FF);
+                    }
+
+                    //channel[2] -= 1000; //Motor
+                    ThrustValue = (channel[2]-1000)*0.001;
+
+                    rollAdjustment = RollPID(rollTarget,rollAngle);
+                    // Limit roll adjustment if needed
+
+                    pitchAdjustment = PitchPID(pitchTarget,pitchAngle);
+                    // Limit pitch adjustment if needed
+
+                    yawAdjustment = YawPID(yawTarget,yawAngle);
+                    // Limit pitch adjustment if needed
+                }
+            }
+            t.stop();
+            pc.printf("The time taken was %.5f seconds\n\r", t.read());
+            UpdateMotors(ThrustValue, yawAngle, pitchAngle, rollAngle);
+        }
+    }
+}
+
+void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
+{
+    //Back Left
+    ESC0 = Thrust + Yaw - Pitch - Roll;
+
+    //Front Left
+    ESC1 = Thrust - Yaw + Pitch - Roll;
+
+    //Front Right
+    ESC2 = Thrust + Yaw + Pitch + Roll;
+
+    //Back Right
+    ESC3 = Thrust + Yaw - Pitch + Roll;
+}
+
+//float MotorCheck(float Motor){
+
+//}
+
+void toggle_led1()
+{
+    ledToggle(1);
+}
+void toggle_led2()
+{
+    ledToggle(2);
+}
+
+/* This function is created to avoid address error that caused from Ticker.attach func */
+void compFilter()
+{
+    mpu6050.complementaryFilter(&pitchAngle, &rollAngle, &yawAngle);
+
+}
\ No newline at end of file