Diff: main.cpp

Revision:

5:d3bdc0672891

Parent:

4:d3c4924179f0

Child:

6:e1e317fe8f7f

--- a/main.cpp	Fri Jun 03 16:58:32 2016 +0000
+++ b/main.cpp	Tue Jun 14 16:02:25 2016 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "HBridgeDCMotor.h"
 #include "FastPWM.h"
+#include "AutomationElements.h"
 
 DigitalIn fault(PTE20);
 DigitalIn sw1(PTC13);
@@ -28,57 +29,90 @@
 PwmOut servo(PTB0);
 
 FastPWM motor1(PTC3);
-DigitalOut mot(PTC4);
+DigitalOut motor1_grana2(PTC4);
 FastPWM motor2(PTC1);
-DigitalOut mot22(PTC2);
+DigitalOut motor2_grana2(PTC2);
+
+Ticker ticker;
+float u_R_a = 0.3, u_R_b = 0.3, x=0;
+double u_i_a = 0, u_i_b = 0;
+double T_d = 1e-3;
+
+PI PI_a (0.04, 0.001, T_d);
+PI PI_b (0.04, 0.001, T_d);
 
+void tick(){
+    u_i_a = 375.0/220.0*(3.3 * a_feedback)/u_R_a;
+    u_i_b = 375.0/220.0*(3.3 * b_feedback)/u_R_b;
+    x = 3 * pot2;
+    PI_a.in(x - u_i_a);
+    PI_b.in(x - u_i_b);
+    u_R_a = PI_a.out();
+    u_R_b = PI_b.out();
+    
+    motor1.pulsewidth(100e-6*u_R_a);
+    motor2.pulsewidth(100e-6*u_R_b); 
 
+    }
 
 void steerServo(float angleDegrees){   
     float minPw = 0.0005, maxPw = 0.0025, minAng = -90, maxAng = 90, pulse = 0;
+    if (angleDegrees < -29 ) angleDegrees = -29;
+    if (angleDegrees > 30 ) angleDegrees = 30;
     pulse = minPw + ((maxPw - minPw)/(maxAng - minAng))*(angleDegrees - minAng);
     servo.pulsewidth(pulse);
     }
 
 int main() {
-    float x=0, pw =0, PWmin=-90, PWmax=90;
+    float pw =0, PWmin=-90, PWmax=90;
     float angleDegrees = 0;
     float sampleTime = 50e-3, switchingFrequency = 5000, rampTime = 3;
     servo.period(1.0/50);
+    PI_a.setOutputLimits (0.3, 1);
+    PI_b.setOutputLimits (0.3, 1);
 
     steerServo(angleDegrees);
-    motor1.period_us(500);
-    motor2.period_us(500);   
+    motor1.period_us(100);
+    motor2.period_us(100);
+    
   //  motor_a.configure(sampleTime, switchingFrequency, rampTime, rampTime);
    // motor_b.configure(sampleTime, switchingFrequency, rampTime, rampTime);
     wait(5);
+    enable = 1;
+    motor2_grana2 = 0;
+    motor1_grana2 = 0;
+    ticker.attach(&tick, T_d);
 
     while(1){
-        x = pot1;
-        angleDegrees = PWmin + (PWmax - PWmin) * pot1;
-enable = 1;
+
+        
+  //      angleDegrees = PWmin + (PWmax - PWmin) * pot1;
+
        // if(sw1==1 && sw2==0) enable = sw1;
       //  else enable = 0;
+      
         
-        mot = 0;
-        mot22 = 0;
-        motor1.pulsewidth_us(100);
+        
 
-        motor2.pulsewidth_us(100);
-            
+        wait(1);
+        
+        
+   /**     motor2_grana2 = 0;
 
 
+        motor2.pulsewidth_us(100*u_R);
+           
        steerServo(angleDegrees);
-       pc.printf("pot1: %f   angleDegrees: %f\r", x, angleDegrees);
+     //  pc.printf("pot1: %f   angleDegrees: %f pot2: %f\r", x, angleDegrees, u_R);
         
     //   motor_a.setDutyCycle(0.2);
      //  motor_b.setDutyCycle(0.2); 
         
         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;
+    //enable = 0;
    // red = 1;