Diff: main.cpp

Revision:

1:50fc6689148e

Parent:

0:1b75c7a92a5e

Child:

2:25b8be806708

--- a/main.cpp	Fri May 06 18:48:56 2016 +0000
+++ b/main.cpp	Thu May 12 08:21:37 2016 +0000
@@ -2,57 +2,57 @@
 #include "HBridgeDCMotor.h"
 
 DigitalIn fault(PTE20);
+AnalogIn a_feedback(PTE23);
+AnalogIn b_feedback(PTE22);
 
-HBridgeDCMotor motor_a(PTC3, PTC4);
-HBridgeDCMotor motor_b(PTC1, PTC2);
+DigitalOut enable(PTE21);
+DigitalOut d1(PTB8);
+DigitalOut d2(PTB9);
+DigitalOut d3(PTB10);
+DigitalOut d4(PTB11);
+//DigitalOut red(PTB18);
+//DigitalOut green(PTB19);
+//DigitalOut blue(PTD1);
+
+//HBridgeDCMotor motor_a(PTC3, PTC4);
+//HBridgeDCMotor motor_b(PTC1, PTC2);
 
 PwmOut servo(PTB0);
-DigitalOut enable(PTE21);
-Timer timer;
+PwmOut motor1(PTC3);
+
+
+void steerServo(float angleDegrees){
+    float offset = 0.00025 * (angleDegrees / 45);
+    if(offset < -0.00025) offset = -0.00025;
+    else if(offset > 0.00025) offset = 0.00025;
+    servo.pulsewidth(0.0010 + offset);
+    }
 
 int main() {
     
-    float sampleTime = 50e-3, switchingFrequency = 5000, rampTime = 3;    
-    timer.start();
-    motor_a.configure(sampleTime, switchingFrequency, rampTime, rampTime);
-    motor_b.configure(sampleTime, switchingFrequency, rampTime, rampTime);
-    servo.period(0.020);
-    float pw=0,pw2=0;
-    servo.pulsewidth(0.00100);
+    float angleDegrees = 0;
+    float sampleTime = 50e-3, switchingFrequency = 5000, rampTime = 3;
+    servo.period(1/50);    
+//    motor_a.configure(sampleTime, switchingFrequency, rampTime, rampTime);
+//    motor_b.configure(sampleTime, switchingFrequency, rampTime, rampTime);
     wait(10);
+    enable = 1;
     while(1){
-
-
-
         
- /**       if( timer.read()> 3 && timer.read()< 9 ){ **/
-             motor_a.setDutyCycle(0.01);
-             motor_b.setDutyCycle(0.01);       
-      /**      }
-        else if( timer.read()> 9 && timer.read()< 15){
-            motor_a.setDutyCycle(-0.01);
-            motor_b.setDutyCycle(-0.01);         
-            }
-        else if (timer.read()>5 && timer.read()<12){
-**/ 
-            for(pw=pw2; pw<0.00025; pw+=0.00001) {
-                servo.pulsewidth(0.001+ pw);
-                wait(0.1);
-                }
-                wait(3);
-            for(pw2=pw; pw2>-0.00025; pw2-=0.00001) {
-            servo.pulsewidth(0.001 + pw2);
-            wait(0.1);
-                }
-                 wait(3);          
- /**           }
-        else {
-            motor_a.setDutyCycle(0);
-            motor_b.setDutyCycle(0); 
-       //     enable=0;
-            } **/
+        motor1.period(0.0002);
+        motor1.write(0.01);
+
+        steerServo(angleDegrees);
+        
+//        motor_a.setDutyCycle(0.1);
+//        motor_b.setDutyCycle(0.1); 
+        
+        if(fault==1) {d1=1; d2=1; d3=1; d4=1;}
+        else {d4=1;d1=1;}      
+
         }
-
-
+    enable = 0;
+   // red = 1;
+    
 
 }