Revision 4:9ecf57487c72, committed 2013-10-18

Comitter:

gerard1993

Date:

Fri Oct 18 09:34:16 2013 +0000

Parent:

3:1241d75b7f49

Child:

5:19687a179088

Commit message:

voor thomas

Changed in this revision

--- a/main.cpp	Tue Oct 15 14:10:02 2013 +0000
+++ b/main.cpp	Fri Oct 18 09:34:16 2013 +0000
@@ -16,42 +16,78 @@
 int main() {
     //LOCAL VARIABLES 
     AnalogIn potmeter(PTC2);
-    Encoder motor1(PTD5,PTC8);//first pin on PTAx or PTDx
     MODSERIAL pc(USBTX,USBRX);
-    PwmOut pwm_motor(PTA5);
-    DigitalOut motordir(PTD1);
+
+    Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx
+    Encoder motor2(PTD5,PTC8);//first pin on PTAx or PTDx
+    PwmOut pwm_motor1(PTA12);
+    PwmOut pwm_motor2(PTA5);
+    DigitalOut motordir1(PTD3);
+    DigitalOut motordir2(PTD1);
+//MOTOR A    
     float setpoint;
-    float pwm_to_motor;
+    float pwm_to_motor1;
     float setspeed;
     float speed;
     float position2;
     float setpoint2;
+//MOTOR B    
+    float setpointB;
+    float pwm_to_motor2;
+    float setspeedB;
+    float speedB;
+    float position2B;
+    float setpoint2B;
+    
     //START OF CODE
     pc.baud(230400);
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,0.01);  
     pc.printf("bla");
+    //A
     speed = 0; 
     position2 = 0;
     setpoint2 = 0;
+    //B
+    speedB = 0; 
+    position2B = 0;
+    setpoint2B = 0;
     //INFINITE LOOP 
     while(1) {
         while(looptimerflag != true);
         looptimerflag = false;
-        setpoint = (potmeter.read()-0.5)*2000;
+        
+//MOTOR A
+        setpoint = (potmeter.read()-0.5)*8000;
         setspeed =(setpoint - setpoint2)/0.01;
         speed = (motor1.getPosition() - position2)/0.01;
         pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition());
-        pwm_to_motor = (setpoint - motor1.getPosition())*.0001 + (setspeed - speed)*.00005 ;
-        keep_in_range(&pwm_to_motor, -1,1);
+        pwm_to_motor1 = (setpoint - motor1.getPosition())*.0001 + (setspeed - speed)*.00005 ;
+        keep_in_range(&pwm_to_motor1, -1,1);
         setpoint2 = setpoint;
         position2 = motor1.getPosition();
-        if(pwm_to_motor > 0)
-            motordir = 1;
+        if(pwm_to_motor1 > 0)
+            motordir1 = 1;
         else
-            motordir = 0;
+            motordir1 = 0;
         //WRITE VALUE TO MOTOR  
-        pwm_motor.write(abs(pwm_to_motor));
+        pwm_motor1.write(abs(pwm_to_motor1));
+        
+//MOTOR B
+        setpointB = (potmeter.read()-0.5)*8000;
+        setspeedB =(setpointB - setpoint2B)/0.01;
+        speedB = (motor2.getPosition() - position2B)/0.01;
+        pc.printf("s: %f, %d \n\r", setpointB, motor2.getPosition());
+        pwm_to_motor2 = (setpointB - motor2.getPosition())*.0001 + (setspeedB - speedB)*.00005 ;
+        keep_in_range(&pwm_to_motor2, -1,1);
+        setpoint2B = setpointB;
+        position2B = motor2.getPosition();
+        if(pwm_to_motor2 > 0)
+            motordir2 = 1;
+        else
+            motordir2 = 0;
+        //WRITE VALUE TO MOTOR  
+        pwm_motor2.write(abs(pwm_to_motor2));
     }
 }