Diff: main.cpp

Revision:

2:91f2479501a0

Parent:

1:0acb5ca94031

Child:

3:1e25f6df1a0d

--- a/main.cpp	Sat May 07 12:05:15 2022 +0000
+++ b/main.cpp	Sun May 08 16:32:55 2022 +0000
@@ -6,7 +6,6 @@
 #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
@@ -17,198 +16,311 @@
 Servo ESC2(p24); //Front Right
 Servo ESC3(p23); //Back  Right
 
+uint16_t motor_min = 1100;
+
+DigitalOut led1(LED1);
+void Rx_interrupt();
+
 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;
+int arm = 0;
+
+uint16_t pitchTarget = 0;
+uint16_t rollTarget = 0;
 //float yawTarget = 0;
 
 float pitchOffset = 0;
 float rollOffset = 0;
 
-//PID PitchPID(0.06,0.05,0);
-//PID RollPID(0.06,0.05,0);
+PID PitchPID(0.5,0,0.05);
+PID RollPID(0.5,0,0.05);
+PID YawPID(2,0.01,0);
+
+float Throttle = 0;
+
+// Receiver variables
+int16_t channel[14];
 
-PID PitchPID(0.15,0.12,0.42);
-PID RollPID(0.15,0.12,0.42);
+float pid_setpoint_roll = 0;
+float pid_setpoint_pitch = 0;
+float pid_setpoint_yaw = 0;
+float pid_output_roll = 0;
+float pid_output_pitch = 0;
+float pid_output_yaw = 0;
+
+int16_t gr[3];
+int16_t acc[3];
+int16_t grC[3];
+int16_t accC[3];
 
 
-//PID YawPID(1,1,0.01);
-
-float Pkc = 0.031;
-float Rkc = 0.031;
-//float Ykc = 0.2;
-
-float ThrustValue = 0;
-
-// Receiver variables
-uint16_t channel[14];
 
 Timer t;
+Timer loop_time;
+Timer re;
 
-void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
-{
-    float ARoll = 0.77*Roll;
-    
-    float Mot0;
-    //Back Left
-    ESC0 = Mot0 = Thrust + Yaw - Pitch - ARoll;
-    
+void Calculate_set_points(){
+	
+	// Throttle- CHannel[2]
+	if (channel[2] > 1800){Throttle = 1800;}
+	else{Throttle = channel[2];}
+	// Roll - Y - Channel[0]
+	if (channel[0] != 0){
+		if (channel[0] > 1505){pid_setpoint_roll =  channel[0] - 1505;} 	// max 495
+		if (channel[0] < 1495){pid_setpoint_roll =  channel[0] - 1495;} 	// max -495
+		pid_setpoint_roll /= 1;	// Max angular rate of 247 deg/s
+	}
+	else {
+		pid_setpoint_roll = 0;
+	}
 	
-	float Mot1;
-    //Front Left
-    ESC1 = Mot1 = Thrust - Yaw + Pitch - ARoll;
-	
-	float Mot2;
-    //Front Right
-    ESC2 = Mot2 = Thrust - Yaw + Pitch + ARoll;
+	// Pitch - X - Channel[1]
+	if (channel[1] != 0){
+		if (channel[1] > 1505){pid_setpoint_pitch = 1505 - channel[1];} 	// max 495
+		if (channel[1] < 1495){pid_setpoint_pitch =  1495 - channel[1];} 	// max -495
+		pid_setpoint_pitch /= 1;	// Max angular rate of 247 deg/s
+	}
+	else {
+		pid_setpoint_roll = 0;
+	}
 	
-	float Mot3;
-    //Back Right
-    ESC3 = Mot3 = Thrust + Yaw - Pitch + ARoll;
-    
-    //pc.printf("BL:%.3f, FL:%.3f, FR:%.3f, BR:%.3f \n\r", Mot0, Mot1, Mot2, Mot3);
-    
-    //pc.printf(" %f\n\r", Mot3);
+	//Yaw - Z - Channel[3]
+	if (channel[3] != 0){
+		if (channel[3] > 1505){pid_setpoint_yaw = 1505 - channel[3];} // max 495
+		if (channel[3] < 1495){pid_setpoint_yaw = 1495 - channel[3];} // max -495
+		pid_setpoint_roll /= 1;		// Max angular rate of 247 deg/s
+	}
+	else{
+		pid_setpoint_yaw = 0;
+		}
+		
+	//pc.printf("Pset: %.1f, Rset %.1f, Yset %.1f ",pid_setpoint_roll, pid_setpoint_roll, pid_setpoint_roll); 
 }
 
-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)
-    wait(1);
-    //rollTarget = rollAngle;
-    //pitchTarget = pitchAngle;
-    //yawTarget = yawAngle;
-
-	rollOffset  = rollAngle;
-	pitchOffset = pitchAngle;
-	
-	//pc.printf("%f ,%f",Roll Angle, pitchAngle)
-	
-    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()) {
+// Interupt Routine to read in data from serial port
+void Rx_interrupt(){
+	uint8_t byte1;
+	uint8_t high_byte;
+	uint8_t chksum_low_byte;
+	uint8_t chksum_high_byte;
+	if (Receiver.readable()) {
             if(Receiver.getc() == 0x20) {
-                if(Receiver.getc() == 0x40) {
-                    //printf("Message Recieved! \n");
+                    if(Receiver.getc() == 0x40){
                     for (int i = 0; i<7; i++) {
-                        byte1 = Receiver.getc();
+                 		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);
-                    }
-                }
-            }
-            //printf("%d\n\r", channel[6]);
-            //channel[6] = 1000;
-			if (channel[6] == 1000){
-                //channel[2] -= 1000; //Motor
-                ThrustValue = (channel[2]-1000)*0.0008;
-
-                rollTarget = -(channel[0]-1500)*0.18 + rollOffset;
-                pitchTarget = -(channel[1]-1500)*0.18+ pitchOffset;
-
-                //pc.printf(" %d %d %d %d \n\r", channel[4], channel[5], channel[6], channel[7]);
-
-                //pc.printf("RT: %f , PT %f \n\r", rollTarget, pitchTarget);
-
-                //pc.printf("%d  , %f \n\r",channel[2],ThrustValue);
-                //ThrustValue = (200)*0.001;
-
-                rollAdjustment = RollPID.computeOutput(rollTarget,rollAngle);
-                rollAdjustment*=Rkc;
-                //printf("Adj: %f, Targ: %f, Ang: %f\n\r",rollAdjustment,rollTarget,rollAngle);
-                // Limit roll adjustment if needed
-
-                pitchAdjustment = PitchPID.computeOutput(pitchTarget,pitchAngle);
-                pitchAdjustment*=Pkc;
-                //printf("Adj: %f, Targ: %f, Ang: %f\n\r",pitchAdjustment,pitchTarget,pitchAngle);
-                //printf("%f\n\r",pitchAdjustment);
-                // Limit pitch adjustment if needed
-
-                //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
-                //yawAdjustment*=Ykc;
-                //printf("Adj: %f, Targ: %f, Ang: %f\n\r",yawAdjustment,yawTarget,yawAngle);
-                //printf("%f\n\r",yawAdjustment);
-                // Limit pitch adjustment if needed
-                t.stop();
-                //pc.printf("The time taken was %.5f seconds\n\r", t.read());
-                //UpdateMotors(ThrustValue, yawAdjustment, pitchAdjustment, rollAdjustment);
-                UpdateMotors(ThrustValue,0, pitchAdjustment, rollAdjustment);
-            }
-            else{
-            	UpdateMotors(0,0,0,0);
+                	}
+                	chksum_low_byte = Receiver.getc();
+                	chksum_high_byte = Receiver.getc();
+                	//pc.printf("CLB %u,CHB %u \n\r", chksum_low_byte, chksum_high_byte);
+                	//pc.printf("CN1: %u, CN2: %u, CN3: %u \n\r", channel[1], channel[2], channel[3]);
+                	}
             }
         }
+}
+
+void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
+{
+    // ESC0 |  Back Left   | CCW
+    // ESC1 |  Front Left  | CW
+    // ESC2 |  Front Right | CCW
+    // ESC3 |  Back Right  | CW
+
+
+    //Back Left
+    float SC0 = 0;
+    SC0 = Thrust - Yaw - Pitch + Roll;
+    if(SC0 < motor_min) {
+        SC0 =  motor_min;
+    }
+    if(SC0 > 2000) {
+        SC0 = 2000;
+    }
+    float NSC0 = (SC0-1003)/1003;
+    ESC0 = NSC0;
+
+    //Front Left
+    float SC1 = Thrust + Yaw + Pitch + Roll;
+    if(SC1 < motor_min) {
+        SC1 =  motor_min;
+    }
+    if(SC1 > 2000) {
+        SC1 = 2000;
+    }
+    float NSC1 = (SC1-1003)/1003;
+    ESC1 = NSC1;
+
+    //Front Right
+    float SC2 = Thrust - Yaw + Pitch - Roll;
+    if(SC2 < motor_min) {
+        SC2 =  motor_min;
+    }
+    if(SC2 > 2000) {
+        SC2 = 2000;
+    }
+    float NSC2 = (SC2-1003)/1003;
+    ESC2 = NSC2;
+
+    //Back Right
+    float SC3 = Thrust + Yaw - Pitch - Roll;
+    if(SC3 < motor_min) {
+        SC3 =  motor_min;
+    }
+    if(SC3 > 2000) {
+        SC3 = 2000;
+    }
+    float NSC3 = (SC3-1003)/1003;
+    ESC3 = NSC3;
+
+
+    //pc.printf("BL:%.3f, FL:%.3f, FR:%.3f, BR:%.3f ", SC0, SC1, SC2, SC3);
+    //pc.printf("NBL:%.3f, NFL:%.3f, NFR:%.3f, NBR:%.3f ", NSC0, NSC1, NSC2, NSC3);
+}
+
+int main()
+{
+    // Setup a serial interrupt function to receive data
+    led1 = 1;
+    Receiver.baud(115200);
+    pc.baud (115200);
+    Receiver.attach(&Rx_interrupt);
+
+    for (int c = 0; c<14; c++) {
+        channel[c] = 0;
+    }
+    ESC0 = ESC1 = ESC2 = ESC3 = 0;
+    wait(0.5);
+    ESC0 = ESC1 = ESC2 = ESC3 = 1100;
+    wait(0.5);
+    ESC0 = ESC1 = ESC2 = ESC3 = 0;
+    wait(0.5);
+    
+
+   //
+    mpu6050.whoAmI();                           // Communication test: WHO_AM_I register reading
+    wait(1);
+    mpu6050.init();    
+    wait(0.5);                         // Initialize the sensor
+    mpu6050.calibrate(accelBias,gyroBias);    // Calibrate MPU6050 and load biases into bias registers
+    pc.printf("Calibration is completed. \r\n");
+    wait(1);
+    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
+    wait_ms(500);
+    
+    
+
+
+    //pc.printf("%f ,%f",Roll Angle, pitchAngle)
+
+    PitchPID.setLimit(-250,250);
+    RollPID.setLimit(-250,250);
+    YawPID.setLimit(-250,250);
+    //YawPID.setLimit(-0.25,0.25);
+
+    pc.printf("Ready to go...\n\r");
+    led1 = 0;
+
+
+
+    while (1) {
+        loop_time.start();
+        t.start();
         
+        // TO ADD
+        // Timer timeout for reciving new commands, if after 10 s no new commands cut motor output
+// AUTO LEVEL 
+		// Read MPU Values with complimantary filter allready applied
+        //mpu6050.complementaryFilter(&pitchAngle, &rollAngle, &yawAngle);
+        //pc.printf("Pitch: %.3f, Roll: %.3f, Yaw: %.3f \n\r", pitchAngle, rollAngle, yawAngle);
+        
+// RATE MODE
+		mpu6050.readAccelData(acc);
+		mpu6050.readGyroData(gr);
+		accC[0] = ((acc[0]-accelBias[0])/16384);
+		grC[0] = ((gr[0]-gyroBias[0])/65.536);
+		
+		accC[1] = ((acc[1]-accelBias[1])/16384);
+		grC[1] = ((gr[1]-gyroBias[1])/65.536);
+		
+		accC[2] = ((acc[2]-accelBias[2])/16384);
+		grC[2] = ((gr[2]-gyroBias[2])/65.536)*-1;
+		
+		pitchAngle =  (pitchAngle*0.7) + (grC[0]*0.3);
+		rollAngle =  (rollAngle*0.7) + (grC[1]*0.3);
+		yawAngle =  (yawAngle*0.7) + (grC[2]*0.3);
+
+		//pc.printf("ac_x: %d ac_y %d ac_z %d gy_x %d gy_y %d gy_z %d  \r\n", accC[0], accC[1], accC[2], grC[0], grC[1], grC[2]);
+        
+        Calculate_set_points();
+        
+        // Set Reciver channels 
+        
+
+        //pc.printf("T:%.1f R:%.1f P:%.1f ARM: %u \n\r",Throttle, pid_setpoint_roll, pid_setpoint_pitch, channel[6]);
+        
+        // Set PIDS
+        pid_output_roll = RollPID.computeOutput(pid_setpoint_roll,rollAngle);
+        pid_output_pitch = PitchPID.computeOutput(pid_setpoint_pitch,pitchAngle);
+        pid_output_yaw = YawPID.computeOutput(pid_setpoint_yaw,yawAngle);
+        //pc.printf("Radj: %.1f, Padj %.1f ",pid_output_roll, pid_output_pitch); 
+        // Yaw angle is currently very broken and constantly rises (Check MPU6050 complimentry filter function as it looks
+        // like it doesnt apply the z axis config offset values)
+        //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
         
         
-        /*
+        // Update Motor PWM if the arming switch is triggered
+        if (channel[6] > 1600){UpdateMotors(Throttle,pid_output_yaw, pid_output_pitch, pid_output_roll);}
         else{
-        	UpdateMotors(0,0,0,0);
-        	pc.printf("Hello");	
-    	}
-    	*/
-    	//wait(0.1);
+        	ESC0 = 0;
+        	ESC1 = 0; 
+        	ESC2 = 0;
+        	ESC3 = 0;
+        }
+        
+
+		
+        
+
+
+
+        
+        
+
+
+
+        //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
+        //yawAdjustment*=Ykc;
+        //printf("Adj: %f, Targ: %f, Ang: %f\n\r",yawAdjustment,yawTarget,yawAngle);
+        //printf("%f\n\r",yawAdjustment);
+        // Limit pitch adjustment if needed
+
+        //if (arm) {}
+
+        t.stop();
+        //pc.printf("Loop time %f \n\r", t.read());
+        if(t.read()*1000 > 3) {
+            pc.printf("LOOP TIME TOO LONG!");
+            led1 = 1;
+        } else {
+            wait_ms(3.04-t.read()*1000);
+        }
+        t.reset();
+        loop_time.stop();
+        //pc.printf("Loop time %f \n\r", loop_time.read());
+        loop_time.reset();
     }
 }
 
@@ -216,18 +328,3 @@
 
 //}
 
-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