Diff: main.cpp

Revision:

5:33133ebe37fd

Parent:

4:8113394bed1b

Child:

6:1e9c8577f7c9

--- a/main.cpp	Mon Oct 14 10:41:49 2019 +0000
+++ b/main.cpp	Fri Oct 18 10:39:08 2019 +0000
@@ -17,14 +17,13 @@
 #include "MODSERIAL.h"
 #include "BiQuad.h"
 #include "FastPWM.h"
-#include "Servo.h"
 
 MODSERIAL pc(USBTX, USBRX);
 
 AnalogIn pot(A1);
 
 // DC MOTORS CODE //
-QEI encoder1(D12,D13,NC,32), encoder2(D10,D11,NC,32), encoder3(D8,D9,NC,32);
+QEI encoder1(D12,D13,NC,32), encoder2(D10,D11,NC,32); // DELETED encoder3 for now because I need D8 for Servo
 PwmOut motor1_pwm(D6), motor2_pwm(D5), motor3_pwm(D3);
 DigitalOut motor1_dir(D7), motor2_dir(D4), motor3_dir(D2);
 int frequency_pwm = 10000;
@@ -103,12 +102,16 @@
 
 
 // SERVO CODE //
-Servo myservo(PTC9);
+PwmOut servo(D9);
 
-
+void moveServo(float i)
+{
+    float duty = (i + 20) / 1000.00;
+    servo.write(duty);
+}
 
 // SOLENOID CODE //
-DigitalOut solenoidA(PTC0), solenoidB(PTC7);
+DigitalOut solenoidA(PTC0), solenoidB(PTC9);
 
 void setSolenoid(int dir)
 {
@@ -126,14 +129,15 @@
     motor1_pwm.period(1.0 / frequency_pwm); // T = 1 / f
     motor2_pwm.period(1.0 / frequency_pwm);
     motor3_pwm.period(1.0 / frequency_pwm);
+    servo.period(0.020);
 
     while (true) {
         /*
         SOME EXAPLE CODE
-        
+
         * MOTOR
         setMotor(..., ...) // which motor (1,2,3), and speed (-inf, +inf)
-        
+
         * PID
         double currentPos1 = encoder1.getPulses();
         double targetPos1 = 10000;
@@ -147,13 +151,20 @@
 
         * SERVO
         myservo = ... // value between 0.0 and 1.0
-        
-        * SOLENOID 
+
+        * SOLENOID
         setSolenoid(0); // shooting
-        wait(1000);
+        wait_ms(1000);
         setSolenoid(1);
-        
+
         */
-      
+        for(int i = 0; i < 100; i++) {
+            moveServo(i);
+            wait_ms(60);
+        }
+        for(int i = 100; i > 0; i--) {
+            moveServo(i);
+            wait_ms(60);
+        }
     }
 }