Diff: main.cpp

Revision:

5:19687a179088

Parent:

4:9ecf57487c72

--- a/main.cpp	Fri Oct 18 09:34:16 2013 +0000
+++ b/main.cpp	Fri Oct 25 11:13:01 2013 +0000
@@ -24,13 +24,16 @@
     PwmOut pwm_motor2(PTA5);
     DigitalOut motordir1(PTD3);
     DigitalOut motordir2(PTD1);
+    
+    pwm_motor1.period(1.0/22000.0);
+    
 //MOTOR A    
     float setpoint;
     float pwm_to_motor1;
     float setspeed;
-    float speed;
-    float position2;
-    float setpoint2;
+//    float speed;
+//    float position2;
+//    float setpoint2;
 //MOTOR B    
     float setpointB;
     float pwm_to_motor2;
@@ -40,14 +43,13 @@
     float setpoint2B;
     
     //START OF CODE
-    pc.baud(230400);
+    pc.baud(115200);
     Ticker looptimer;
-    looptimer.attach(setlooptimerflag,0.01);  
-    pc.printf("bla");
+    looptimer.attach(setlooptimerflag,0.001);  
     //A
-    speed = 0; 
-    position2 = 0;
-    setpoint2 = 0;
+//    speed = 0; 
+//    position2 = 0;
+//    setpoint = 0;
     //B
     speedB = 0; 
     position2B = 0;
@@ -58,14 +60,11 @@
         looptimerflag = false;
         
 //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_motor1 = (setpoint - motor1.getPosition())*.0001 + (setspeed - speed)*.00005 ;
+        setspeed = (potmeter.read()-0.5)*.0001;
+        setpoint = setpoint + setspeed;
+        pwm_to_motor1 = (setpoint - (motor1.getPosition()/4128))*20 + (setspeed - (motor1.getSpeed()/4128))*1.4 ;
         keep_in_range(&pwm_to_motor1, -1,1);
-        setpoint2 = setpoint;
-        position2 = motor1.getPosition();
+        
         if(pwm_to_motor1 > 0)
             motordir1 = 1;
         else
@@ -76,7 +75,7 @@
 //MOTOR B
         setpointB = (potmeter.read()-0.5)*8000;
         setspeedB =(setpointB - setpoint2B)/0.01;
-        speedB = (motor2.getPosition() - position2B)/0.01;
+        speedB = (motor2.getPosition() - position2B)/0.001;
         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);