Diff: main.cpp

Revision:

4:d3c4924179f0

Parent:

3:fc17c2ad6b81

Child:

5:d3bdc0672891

--- a/main.cpp	Wed May 18 20:26:32 2016 +0000
+++ b/main.cpp	Fri Jun 03 16:58:32 2016 +0000
@@ -1,9 +1,17 @@
 #include "mbed.h"
 #include "HBridgeDCMotor.h"
+#include "FastPWM.h"
 
 DigitalIn fault(PTE20);
+DigitalIn sw1(PTC13);
+DigitalIn sw2(PTC17);
 AnalogIn a_feedback(PTE23);
 AnalogIn b_feedback(PTE22);
+AnalogIn pot1(PTB3);
+AnalogIn pot2(PTB2);
+
+Serial pc(PTA2, PTA1);
+
 
 DigitalOut enable(PTE21);  
 DigitalOut d1(PTB8); 
@@ -19,44 +27,55 @@
 
 PwmOut servo(PTB0);
 
-PwmOut motor1(PTC3);
+FastPWM motor1(PTC3);
 DigitalOut mot(PTC4);
-DigitalOut mot2(PTC1);
+FastPWM motor2(PTC1);
 DigitalOut mot22(PTC2);
 
 
 
 void steerServo(float angleDegrees){   
-    float minPw = 0.00750, maxPw = 0.00125, minAng = -45, maxAng = 45, pulse = 0;
+    float minPw = 0.0005, maxPw = 0.0025, minAng = -90, maxAng = 90, pulse = 0;
     pulse = minPw + ((maxPw - minPw)/(maxAng - minAng))*(angleDegrees - minAng);
     servo.pulsewidth(pulse);
     }
 
 int main() {
-    
+    float x=0, pw =0, PWmin=-90, PWmax=90;
     float angleDegrees = 0;
     float sampleTime = 50e-3, switchingFrequency = 5000, rampTime = 3;
     servo.period(1.0/50);
-    servo.pulsewidth(0.0010);
-   motor1.period(0.0001);    
- //   motor_a.configure(sampleTime, switchingFrequency, rampTime, rampTime);
-  //  motor_b.configure(sampleTime, switchingFrequency, rampTime, rampTime);
+
+    steerServo(angleDegrees);
+    motor1.period_us(500);
+    motor2.period_us(500);   
+  //  motor_a.configure(sampleTime, switchingFrequency, rampTime, rampTime);
+   // motor_b.configure(sampleTime, switchingFrequency, rampTime, rampTime);
     wait(5);
-    enable = 1;
+
     while(1){
-     
+        x = pot1;
+        angleDegrees = PWmin + (PWmax - PWmin) * pot1;
+enable = 1;
+       // if(sw1==1 && sw2==0) enable = sw1;
+      //  else enable = 0;
+        
         mot = 0;
-        mot2 = 0;
         mot22 = 0;
-        motor1.pulsewidth(0.00003);
+        motor1.pulsewidth_us(100);
 
-        steerServo(angleDegrees);
+        motor2.pulsewidth_us(100);
+            
+
+
+       steerServo(angleDegrees);
+       pc.printf("pot1: %f   angleDegrees: %f\r", x, angleDegrees);
         
- //       motor_a.setDutyCycle(0.2);
- //       motor_b.setDutyCycle(0.2); 
+    //   motor_a.setDutyCycle(0.2);
+     //  motor_b.setDutyCycle(0.2); 
         
-        if(fault==1) {d1=1; d2=1; d3=1; d4=1;}
-        else {d4=1;d1=1;}      
+        if(x<0.501 && x>0.499) {d1=1; d2=1; d3=1; d4=1;}
+        else {d1=1; d2=0; d3=0; d4=1;}      
 
         }
     enable = 0;