Diff: main.cpp

Revision:

23:0c02cf961344

Parent:

22:71524e4fd1f2

--- a/main.cpp	Mon Oct 29 11:35:49 2018 +0000
+++ b/main.cpp	Mon Oct 29 12:31:55 2018 +0000
@@ -24,12 +24,12 @@
 DigitalIn pin13(D13);   // Encoder 3 A
 
 // Output
-DigitalOut pin2(D2);    // Motor 3 direction
-FastPWM pin3(D3);       // Motor 3 pwm
-DigitalOut pin4(D4);    // Motor 2 direction
-FastPWM pin5(D5);       // Motor 2 pwm
-FastPWM pin6(D6);       // Motor 1 pwm
-DigitalOut pin7(D7);    // Motor 1 direction
+DigitalOut  pin2(D2);    // Motor 3 direction
+FastPWM     pin3(D3);       // Motor 3 pwm
+DigitalOut  pin4(D4);    // Motor 2 direction
+FastPWM     pin5(D5);       // Motor 2 pwm
+FastPWM     pin6(D6);       // Motor 1 pwm
+DigitalOut  pin7(D7);    // Motor 1 direction
 //float u1  = pot1;
 
 // Utilisation of libraries
@@ -38,12 +38,29 @@
 QEI         Encoder2(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
 QEI         Encoder3(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
 Ticker      motor;
+Ticker      encoder;
 
 // Global variables
-const float    pi = 3.14159265358979;
+const float     pi = 3.14159265358979;
+const double    dCountsRad = 4200.0;
+const float     fCountsRad = 4200.0;
 double u3 = 0.0;         // Normalised variable for the movement of motor 3
 
+
 // Functions
+/*float   AngleCalc(float counts)
+{   float angle = ((float)counts*2.0*pi)/fCountsRad;
+    /*if (angle > 2.0*pi)
+    {   angle = angle - 2.0*pi;
+    }
+    else if (angle < 2.0*pi)
+    {   angle = angle + 2.0*pi;
+    }
+    
+    return angle;
+}
+*/
+
 void Encoderinput()
 {   int counts1;
     int counts2;
@@ -54,85 +71,80 @@
     counts1 = Encoder1.getPulses();
     counts2 = Encoder2.getPulses();
     counts3 = Encoder3.getPulses();
-    angle1 = ((float)counts1*2.0*pi)/4200.0;
-    angle2 = ((float)counts2*2.0*pi)/4200.0;
-    angle3 = ((float)counts3*2.0*pi)/4200.0;
-         
-    pc.printf("Counts1,2,3: %i %i %i  Angle1,2,3: %f %f %f  \r\n",counts1,counts2,counts3,angle1,angle2,angle3);
+    //angle1 = AngleCalc (counts1);
+    //angle2 = AngleCalc (counts2);
+    //angle3 = AngleCalc (counts3);
+    angle1 = ((float)counts1*2.0*pi)/fCountsRad;
+    angle2 = ((float)counts2*2.0*pi)/fCountsRad;
+             
+    pc.printf("Counts1,2,3: %i  %i  %i  Angle1,2,3: %f  %f  %f  \r\n",counts1,counts2,counts3,angle1,angle2,angle3);
 }
 
-void draaibuttons()         
-{   /*  Pressing button 2 concludes in a change of speed. While button 1 is pressed,
-        the direction of change of speed is reversed. So pressing button 1 and 2
-        simultaneously results for the turning speed of motor 3 in a slower movement,
-        and eventually the motor will turn the other way around.
-    */
-    if (button1 == 1 && button2 == 1)
-        {   u3 = u3 + 0.1f;  //In stapjes van 0.1            
-            if (u3>1.0f)
-            {   u3 = 1.0f;
-            }
-        }
+/*double RefVelocity()
+{   // Returns reference velocity in rad/s. 
+    // Positive value means clockwise rotation.
+    const float maxVelocity=8.4; // in rad/s of course!    
+    double RefVelocity;  // in rad/s
     
-    else if (button1 == 0 && button2 == 1)
-        {   u3 = u3 - 0.1f;
-            if (u3>1.0f)
-            {   u3 = 1.0f;
-            }
-        }
+    if (button1 == 1)   
+    {   // Clockwise rotation      
+        RefVelocity = potMeterIn * maxVelocity; 
+    } 
+    else
+    {   // Counterclockwise rotation       
+        RefVelocity = -1*potMeterIn * maxVelocity;   
+    }
+    return RefVelocity;
 }
+
+double ActualPosition()
+{   
+    double MotorPos = - (counts1 - offsetcounts1) / dCountsRad;
+    // minus sign to correct for direction convention
+    return MotorPos;
+}
+*/
     
-void draai()    
+void turn()    
 /*  Function for the movement of all motors, using the potmeters for the moving
     direction and speed of motor 1 and 2, and using button 1 and 2 on the biorobotics
     shield for the moving direction and speed of motor 3.
 */
 {      
-    double u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 1
+    double u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 2
     if (u1>0)
     {   pin4 = true;
     }
     else if(u1<0)
     {   pin4 = false;    
     }
-    pin5.period(0.2);   // Set PWM period to 0.2 seconds
-    pin5 = fabs(u1);
-    //pin5.write(fabs(u1));    // Set duty cycle (and thus the angle of the motor) to potmeter percentage
+    pin5.period(fabs(u1));   // Set PWM period to 0.2 seconds
+    //pin5 = fabs(u1);
+    //float a1=fabs(u1);
+    //pin5.write(a1);    // Set duty cycle (and thus the angle of the motor) to potmeter percentage
     
-    double u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
+    double u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 1
     if (u2<0)
     {   pin7 = true;
     }
     else if(u2>0)
     {   pin7 = false;    
     }
-    pin6.period(0.2);   // Set PWM period to 0.2 seconds
-    //pin6.write(fabs(u2));    // Set output duty cycle (and thus the angle of the motor) to potmeter percentage
-    
-    if (u3>0)
-    {   pin2 = true;
-    }
-    else if(u3<0)
-    {   pin2 = false; 
-    }
-    else
-    {   pin3 = 0;
-    }
-    pin3.period(0.2);   // Set PWM period to 0.2 seconds
-    //pin3.write(fabs(u3));    // Set duty cycle (and thus the angle of the motor) to potmeter percentage   
+    pin6.period(fabs(u2));   // Set PWM period to 0.2 seconds
+    //float a2 = fabs(u2);
+    //pin6.write(a2);    // Set output duty cycle (and thus the angle of the motor) to potmeter percentage
 }
 
 // Main program
 int main()
 {   
     pc.baud(115200);
-    
-    button1.rise(&draaibuttons);       
-    button2.rise(&draaibuttons);
-    
-    motor.attach(draai, 0.001);
+        
+    motor.attach(turn, 0.001);
+    //encoder.attach(Encoderinput,0.01);
         
     while   (true)
-    {   Encoderinput();
+    {   // 
+        Encoderinput();
     }
 }
\ No newline at end of file