Diff: main.cpp

Revision:

14:988b8d294259

Parent:

12:0fb3a03aeecb

--- a/main.cpp	Mon Feb 06 20:38:25 2017 +0000
+++ b/main.cpp	Sun Aug 23 18:43:13 2020 +0000
@@ -126,16 +126,22 @@
     MAX32630FTHR pegasus;
     pegasus.init(MAX32630FTHR::VIO_3V3);
     
-    static const float MAX_PULSEWIDTH_US = 40.0F;
-    static const float MIN_PULSEWIDTH_US = -40.0F;
+    static const float MAX_PULSEWIDTH_US = 50.0F;
+    static const float MIN_PULSEWIDTH_US = -50.0F;    
+    static const int PERIOD_US = 100;
+    static const int MAX_PULSEWIDTH = 50;
     
     static const bool FORWARD = 0;
     static const bool REVERSE = 1;
        
     //Setup left motor(from driver seat)
-    DigitalOut leftDir(P5_6, FORWARD);
+    //DigitalOut leftDir(P5_6, FORWARD);    
+    PwmOut leftDir(P5_6);
+    leftDir.period_us(PERIOD_US);
+    leftDir.pulsewidth_us(0);
+    
     PwmOut leftPwm(P4_0);
-    leftPwm.period_us(40);
+    leftPwm.period_us(PERIOD_US);
     leftPwm.pulsewidth_us(0);
     
     //Make sure P4_2 and P4_3 are Hi-Z
@@ -143,9 +149,13 @@
     DigitalIn p43_hiZ(P4_3, PullNone);
     
     //Setup right motor(from driver seat)
-    DigitalOut rightDir(P5_4, FORWARD);
+    //DigitalOut rightDir(P5_4, FORWARD);
+    PwmOut rightDir(P5_4);
+    rightDir.period_us(PERIOD_US);
+    rightDir.pulsewidth_us(0);
+    
     PwmOut rightPwm(P5_5);
-    rightPwm.period_us(40);
+    rightPwm.period_us(PERIOD_US);
     rightPwm.pulsewidth_us(0);
     
     //Turn off RGB LEDs
@@ -318,14 +328,14 @@
         bool firstCal = true;
         
         //Callbacks for echo measurement
-        pingEchoRiseInt.fall(&echoStartISR);
-        pingEchoFallInt.rise(&echoStopISR);
+        //pingEchoRiseInt.fall(&echoStartISR);
+        //pingEchoFallInt.rise(&echoStopISR);
         
         //Timer for echo measurements  
-        pingTimer.start();
+        //pingTimer.start();
         
         //Timer for trigger
-        pingTriggerTimer.attach(&triggerPing, 0.05);
+        //pingTriggerTimer.attach(&triggerPing, 0.05);
         
         //Position control vars/constants
         //static const float PING_SP = 20.0F;
@@ -379,7 +389,7 @@
             
             
             while(start && (pitch > -20.0F) && (pitch < 20.0F))
-            {
+            {                
                 rLED = LED_OFF;
                 gLED = LED_ON;
                 
@@ -429,33 +439,33 @@
                     //based on sign of error signal (negation of pitch since setPoint = 0)
                     if(pulseWidth > MAX_PULSEWIDTH_US)
                     {
-                        rightDir = FORWARD;
-                        leftDir = FORWARD;
-                        pulseWidth = 40.0F;
-                        rightPwm.pulsewidth_us(40);
-                        leftPwm.pulsewidth_us(40);
+                        //rightDir = FORWARD;
+                        //leftDir = FORWARD;
+                        pulseWidth = MAX_PULSEWIDTH_US;
+                        rightPwm.pulsewidth_us(MAX_PULSEWIDTH);
+                        leftPwm.pulsewidth_us(MAX_PULSEWIDTH);
                     }
                     else if(pulseWidth < MIN_PULSEWIDTH_US)
                     {
-                        rightDir = REVERSE;
-                        leftDir = REVERSE;
-                        pulseWidth = -40.0F;
-                        rightPwm.pulsewidth_us(40);
-                        leftPwm.pulsewidth_us(40);
+                        //rightDir = REVERSE;
+                        //leftDir = REVERSE;
+                        pulseWidth = MIN_PULSEWIDTH_US;
+                        rightDir.pulsewidth_us(MAX_PULSEWIDTH);
+                        leftDir.pulsewidth_us(MAX_PULSEWIDTH);
                     }
                     else
                     {
                         if(pulseWidth < 0.0F)
                         {
-                            rightDir = REVERSE;
-                            leftDir = REVERSE;
-                            rightPwm.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
-                            leftPwm.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
+                            //rightDir = REVERSE;
+                            //leftDir = REVERSE;
+                            rightDir.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
+                            leftDir.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
                         }
                         else
                         {
-                            rightDir = FORWARD;
-                            leftDir = FORWARD;
+                            //rightDir = FORWARD;
+                            //leftDir = FORWARD;
                             rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
                             leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
                         }
@@ -488,6 +498,8 @@
             previousError = 0.0F;
             rightPwm.pulsewidth_us(0);
             leftPwm.pulsewidth_us(0);
+            rightDir.pulsewidth_us(0);
+            leftDir.pulsewidth_us(0);
             
             rLED = LED_ON;
             gLED = LED_ON;