Revision 1:0acb5ca94031, committed 2022-05-07

Comitter:

Forest1213

Date:

Sat May 07 12:05:15 2022 +0000

Parent:

0:7f6319a73219

Child:

2:91f2479501a0

Commit message:

Latest Code

Changed in this revision

--- a/main.cpp	Thu Apr 28 23:16:23 2022 +0000
+++ b/main.cpp	Sat May 07 12:05:15 2022 +0000
@@ -35,11 +35,23 @@
 
 float pitchTarget = 0;
 float rollTarget = 0;
-float yawTarget = 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,1,0.01);
-PID RollPID(0.5,1,0.01);
-PID YawPID(0.5,1,0.01);
+PID PitchPID(0.15,0.12,0.42);
+PID RollPID(0.15,0.12,0.42);
+
+
+//PID YawPID(1,1,0.01);
+
+float Pkc = 0.031;
+float Rkc = 0.031;
+//float Ykc = 0.2;
 
 float ThrustValue = 0;
 
@@ -48,6 +60,32 @@
 
 Timer t;
 
+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;
+    
+	
+	float Mot1;
+    //Front Left
+    ESC1 = Mot1 = Thrust - Yaw + Pitch - ARoll;
+	
+	float Mot2;
+    //Front Right
+    ESC2 = Mot2 = Thrust - Yaw + Pitch + ARoll;
+	
+	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);
+}
+
 int main()
 {
     ESC0 = ESC1 = ESC2 = ESC3 = 0;
@@ -74,19 +112,25 @@
     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;
+    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);
+    //YawPID.setLimit(-0.25,0.25);
 
     pc.printf("Ready to go...\n\r");
 
     float rollAdjustment = 0;
     float pitchAdjustment = 0;
-    float yawAdjustment = 0;
+    //float yawAdjustment = 0;
 
     while (1) {
         t.start();
@@ -104,7 +148,7 @@
             if(Receiver.getc() == 0x20) {
                 if(Receiver.getc() == 0x40) {
                     //printf("Message Recieved! \n");
-                    for (int i = 0; i<3; i++) {
+                    for (int i = 0; i<7; i++) {
                         byte1 = Receiver.getc();
                         //byte2 = Receiver.getc();
                         //if (i == 2)
@@ -112,40 +156,60 @@
                         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);
-        }
-    }
-}
+            //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
 
-void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
-{
-    //Back Left
-    ESC0 = Thrust + Yaw - Pitch - Roll;
+                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
 
-    //Front Left
-    ESC1 = Thrust - Yaw + Pitch - Roll;
-
-    //Front Right
-    ESC2 = Thrust + Yaw + Pitch + Roll;
-
-    //Back Right
-    ESC3 = Thrust + Yaw - Pitch + Roll;
+                //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);
+            }
+        }
+        
+        
+        
+        /*
+        else{
+        	UpdateMotors(0,0,0,0);
+        	pc.printf("Hello");	
+    	}
+    	*/
+    	//wait(0.1);
+    }
 }
 
 //float MotorCheck(float Motor){