Diff: main.cpp

Revision:

1:a286bf92d291

Parent:

0:ef6268629f0b

Child:

2:7affec1c81cb

--- a/main.cpp	Fri Sep 28 10:46:57 2018 +0000
+++ b/main.cpp	Thu Oct 11 21:26:53 2018 +0000
@@ -15,6 +15,7 @@
 static const int NUMBER_OF_MOTORS = 3;
 
 PwmOut m3(M3_PWM);
+DigitalOut servo(ISO_PWM5);
 
 Motor motor0(M0_PWM, M0_DIR1, M0_DIR2, M0_ENCA, M0_ENCB);
 Motor motor1(M1_PWM, M1_DIR1, M1_DIR2, M1_ENCA, M1_ENCB);
@@ -42,6 +43,10 @@
 int failSafeCount = 0;
 int failSafeLimit = 60;
 
+Ticker softPWMTicker;
+Timeout softPWMTimeout;
+int currentServoPosition = 0;
+
 void pidTick() {
     for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
         motors[i]->pid();
@@ -66,13 +71,33 @@
     }
 }
 
+void softPWMEndPulse() {
+    servo = 0;
+}
+
+void softPWMTick() {
+    if(currentServoPosition <= 0) {
+        servo = 0 ;
+    } else {
+        servo = 1;
+        softPWMTimeout.attach_us(softPWMEndPulse, currentServoPosition);
+    }
+}
+
 int main() {
     pidTicker.attach(pidTick, 1 / PID_FREQ);
     //serial.attach(&serialInterrupt);
 
-    // Dribbler motor
-    //m3.period_us(4000);
+    //servo.period_us(20000);
+
     m3.pulsewidth_us(800);
+    //servo.pulsewidth_us(0);
+    
+    // TGY-180D (KC2462 controller) has problems with higher frequency PWM.
+    // 50Hz works, but would like to use higher frequency for motors.
+    // Software PWM seems to be good enough.
+    
+    softPWMTicker.attach_us(softPWMTick, 20000);
 
     while (1) {
         if (rfModule.readable()) {
@@ -111,33 +136,39 @@
             motors[1]->getSpeed(),
             motors[2]->getSpeed()
          );
-    }
-
-    if (strncmp(cmd, "d", 1) == 0) {
+         
+    } else if (strncmp(cmd, "d", 1) == 0) {
         int pulsewidth = atoi(buffer + 2);
         
         if (pulsewidth < 800) {
             pulsewidth = 800;
         } else if (pulsewidth > 2100) {
-           pulsewidth = 2100;
+            pulsewidth = 2100;
         }
         
-        m3.pulsewidth_us(pulsewidth);        
-    }
-
-    if (strncmp(cmd, "gs", 2) == 0) {
+        m3.pulsewidth_us(pulsewidth);
+             
+    } else if (strncmp(cmd, "sv", 2) == 0) {
+        currentServoPosition = atoi(buffer + 3);
+        
+        if (currentServoPosition < 700) {
+            currentServoPosition = 0;
+        } else if (currentServoPosition > 2300) {
+            currentServoPosition = 2300;
+        }
+        
+        //servo.pulsewidth_us(currentServoPosition);
+             
+    } else if (strncmp(cmd, "gs", 2) == 0) {
         serial.printf("<gs:%d:%d:%d>\n",
             motors[0]->getSpeed(),
             motors[1]->getSpeed(),
             motors[2]->getSpeed()
         );
-    }
-
-    if (strncmp(cmd, "rf", 2) == 0) {
+    } else if (strncmp(cmd, "rf", 2) == 0) {
         rfModule.send(buffer + 3);
-    }
-
-    if (strncmp(cmd, "fs", 1) == 0) {
+        
+    } else if (strncmp(cmd, "fs", 1) == 0) {
         failSafeEnabled = buffer[3] != '0';
     }
 }