Diff: setup.cpp

Revision:

144:b3a713b4f1c4

Parent:

143:53808e4e684c

--- a/setup.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/setup.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -27,34 +27,60 @@
     float magMax[3] =  {182.602386, 530.811523, 365.834625};
     magCalibrator.setExtremes(magMin,magMax);
     
-    pitchPID.setSetPoint(0.0f);
-    pitchratePID.setSetPoint(0.0f); 
-    rollPID.setSetPoint(0.0f); 
-    rollratePID.setSetPoint(0.0f); 
-    pitchPID.setBias(0.0f);
-    pitchratePID.setBias(0.0f); 
-    rollPID.setBias(0.0f); 
-    rollratePID.setBias(0.0f); 
-    pitchPID.setOutputLimits(-1.0f,1.0f);
-    pitchratePID.setOutputLimits(-1.0f,1.0f);
-    rollPID.setOutputLimits(-1.0f,1.0f); 
-    rollratePID.setOutputLimits(-1.0f,1.0f);
-    pitchPID.setInputLimits(-M_PI_F,M_PI_F);
-    pitchratePID.setInputLimits(-M_PI_F,M_PI_F);
-    rollPID.setInputLimits(-M_PI_F,M_PI_F); 
-    rollratePID.setInputLimits(-M_PI_F,M_PI_F);
+    pitchPID.setSetPoint(0.0);
+    pitchPID.setBias(0.0);
+    pitchPID.setOutputLimits(-1.0,1.0);
+    pitchPID.setInputLimits(-M_PI,M_PI);
+    
+    pitchratePID.setSetPoint(0.0); 
+    pitchratePID.setBias(0.0); 
+    pitchratePID.setOutputLimits(-1.0,1.0);
+    pitchratePID.setInputLimits(-M_PI,M_PI);
+    
+    rollPID.setSetPoint(0.0); 
+    rollPID.setBias(0.0); 
+    rollPID.setOutputLimits(-1.0,1.0);
+    rollPID.setInputLimits(-M_PI,M_PI); 
+     
+    rollratePID.setSetPoint(0.0);
+    rollratePID.setBias(0.0);
+    rollratePID.setOutputLimits(-1.0,1.0);
+    rollratePID.setInputLimits(-M_PI,M_PI);
+    
+    yawratePID.setSetPoint(0.0); 
+    yawratePID.setBias(0.0); 
+    yawratePID.setOutputLimits(-1.0,1.0);
+    yawratePID.setInputLimits(-M_PI,M_PI);
     
-    servoRight.period_us(15000.0f);
-    servoLeft.period_us(15000.0f);
-    servoThrust.period_us(15000.0f);
-    servoRight.pulsewidth_us(1500.0f);
-    servoLeft.pulsewidth_us(1500.0f); 
-    servoThrust.pulsewidth_us(1100.0f);
+    vxPID.setSetPoint(0.0f); 
+    vxPID.setBias(0.0f); 
+    vxPID.setOutputLimits(-1.0f,1.0);
+    vxPID.setInputLimits(-5.0f,5.0f);
+    
+    vyPID.setSetPoint(0.0f); 
+    vyPID.setBias(0.0f); 
+    vyPID.setOutputLimits(-1.0f,1.0);
+    vyPID.setInputLimits(-5.0f,5.0f);
+    
+    vzPID.setSetPoint(0.0f); 
+    vzPID.setBias(0.0f); 
+    vzPID.setOutputLimits(-1.0f,1.0);
+    vzPID.setInputLimits(-1.0f,1.0f);
     
-    autopilot.set_dest(0.0f, 50.0f);
-    autopilot.set_turn(0.0f, 50.0f, 50.0f);
-    autopilot.set_alt(30.0f, 10.0f);
-    autopilot.set_climb(3.0f*M_PI_F/180.0f, 0.0f);
+    
+    motor1.period_us(15000.0);
+    motor2.period_us(15000.0);
+    motor3.period_us(15000.0);
+    motor4.period_us(15000.0);
+    motor5.period_us(15000.0);
+    motor6.period_us(15000.0);
+
+    motor1.pulsewidth_us(motorPwmMin);
+    motor2.pulsewidth_us(motorPwmMin);
+    motor3.pulsewidth_us(motorPwmMin);
+    motor4.pulsewidth_us(motorPwmMin);
+    motor5.pulsewidth_us(motorPwmMin);
+    motor6.pulsewidth_us(motorPwmMin);
     
 }