Diff: main.cpp

Revision:

25:86741f4565f1

Parent:

17:16d29ed4ab00

Child:

26:21911b4a79b4

--- a/main.cpp	Fri Oct 04 10:17:19 2019 +0000
+++ b/main.cpp	Mon Oct 07 12:48:52 2019 +0000
@@ -28,6 +28,20 @@
 
     //PC
         Serial pc(USBTX,USBRX);
+        
+    //Structure
+        struct struc1
+        {
+            int calibrationCountsMotor1;
+            int calibrationCountsMotor2;
+            int calibrationCountsMotor3;
+            
+            float countsMotor1Return;
+            float countsMotor2Return;
+            float velocityMotor1Return;
+            float velocityMotor2Return;
+        };
+        struc1 strucM&E
 
 //Variables
     enum States {MovementIdle, CalibrationIdle, Demo, Startup, CalibrationPhysical, CalibrationEMG, Move, TiltCup, FailState};
@@ -36,6 +50,7 @@
     volatile char ledcolor; //r is red, b is blue, g is green, t is bluegreen, p is purple
     volatile int errorCode;
     
+        
     //Ticker Timings
         const float mainLoopT = 2; //Main Loopt Ticker wait
         const float ledFlipperT = .5; //LED Flicker wait
@@ -154,7 +169,45 @@
 void Run_CalibrationPhysical(void)
 {
     pc.printf("Starting Calibration Physical ... \r\n");
-    wait(1);
+    float calibrationPeriodMotor1 = 1/2000; 
+    float calibrationPeriodMotor2 = 1/2000;
+    float calibrationPeriodMotor3 = 1/2000; 
+    
+    float calibrationPWM = .1;
+    float calibrationVelocity = .1;
+    
+    float calibrationPWM1 = calibrationPWM;
+    float calibrationPWM2 = 0;
+    float calibrationPWM3 = 0;
+    
+    motorAndEncoder(calibrationPWM1, calibrationPeriodMotor1, calibrationPWM2, calibrationPeriodMotor2, mainLoopT,  &countsMotor1Return, &countsMotor2Return, &velocityMotor1Return, &velocityMotor2Return, calibrationCountsMotor1, calibrationCountsMotor2, calibrationCountsMotor3);
+    
+    if (velocityMotor1 <= calibrationVelocity)
+    {
+        calibrationCountsMotor1 = countsMotor1Return; //calibrate motor1
+        calibrationPWM1 = 0;
+        calibrationPWM2 = calibrationPWM;
+        calibrationPWM3 = 0;
+        if (velocityMotor2 <= calibrationVelocity) //Calibrate motor2
+        {
+            calibrationCountsMotor2 = countsMotor2Return;
+            calibrationPWM1 = 0;
+            calibrationPWM2 = 0;
+            calibrationPWM3 = calibrationPWM;
+            
+            if (velocityMotor3 <= calibrationVelocity) //calibrate motor3
+            {
+                calibrationCountsMotor3 = countsMotor3Return;
+                calibrationPWM1 = 0;
+                calibrationPWM2 = 0;
+                calibrationPWM3 = 0;
+                motorAndEncoder(calibrationPWM1, calibrationPeriodMotor1, calibrationPWM2, calibrationPeriodMotor2, mainLoopT,  float &countsMotor1Return, float &countsMotor2Return, float &velocityMotor1Return, float &velocityMotor2Return, calibrationCountsMotor1, calibrationCountsMotor2, calibrationCountsMotor3);
+            }
+        }
+        
+    }
+    
+    
     CurrentState = CalibrationIdle;
     
 }