Diff: main.cpp

Revision:

3:967aee5fed5b

Parent:

2:011e6115c77a

Child:

4:8fcaff7801b0

--- a/main.cpp	Mon Dec 02 02:04:30 2013 +0000
+++ b/main.cpp	Mon Dec 02 02:22:37 2013 +0000
@@ -62,21 +62,37 @@
     }
 }
 
+void setVoltage(float inputVoltage)
+{
+    if(inputVoltage<0.0) {
+        inputVoltage = -inputVoltage;
+        dOut1=0;
+        dOut2=1;
+    } else {
+        dOut1=1;
+        dOut2=0;
+    }
+    float dutyCycle = inputVoltage/MAX_VOLTAGE;
+    dutyCycle = (dutyCycle > 1.0)? 1.0 : dutyCycle;
+    pwmOut.write(dutyCycle);
+}
+
 void computing(void const *args) {
     float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0;
-    float freq = 1.0, dutyCycle;
+    float freq = 2.0, inputVoltage;
     int pulsesPend, pulsesMot;
     while (true) {
         t = T.read();
         
         //set pwm 
-        dutyCycle = (sin(freq*t));
-        dutyCycle = dutyCycle*dutyCycle;
+        inputVoltage = MAX_VOLTAGE * (sin(freq*t));
         //pc.printf("Duty%f\n\r",dutyCycle);
-        pwmOut.write(dutyCycle);
+        setVoltage(inputVoltage);
         
         //read current
         mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP; 
+        if(dOut1 == 0)
+            mCurrent = -mCurrent;
         pulsesPend = -encoder.getPulses();
         pulsesMot = motor.getPulses();