Diff: main.cpp

Revision:

1:14b685c3abbd

Parent:

0:1b8a08e9a66c

Child:

2:e36213affbda

--- a/main.cpp	Sat Oct 07 20:46:58 2017 +0000
+++ b/main.cpp	Thu Oct 12 21:01:29 2017 +0000
@@ -1,48 +1,62 @@
 #include "mbed.h"
-//#include "QEI.h"
+#include "QEI.h"
 #include "MODSERIAL.h"
 //#include "HIDScope.h"
 
 
 Serial pc (USBTX,USBRX);
-//PwmOut Motor1Vel(D6);            //motor 1 velocity control
-//DigitalOut Motor1Dir(D7);        //motor 1 direction
-//PwmOut Motor2Vel(D5);            //motor 2 velocity control
-//DigitalOut Motor2Dir(D4);        //motor 2 direction
-DigitalOut led1(D8);
-DigitalOut led2(D9);
+
+PwmOut Motor1Vel(D6);            //motor 1 velocity control
+DigitalOut Motor1Dir(D7);        //motor 1 direction
+PwmOut Motor2Vel(D5);            //motor 2 velocity control
+DigitalOut Motor2Dir(D4);        //motor 2 direction
+DigitalOut led1(D2);
+DigitalOut led2(D3);
 AnalogIn emgx(A0);
 AnalogIn emgy(A1);
+//DigitalOut Button(D10);
 //DigitalOut ledb(LED1);
 
+QEI encoder1(D8,D9,NC,16);
+QEI encoder2(D10,D11,NC,16);
+
 Ticker switch_tick;
+const float pi = 3.14159265359;   //to convert pulses to radians 
+const float PulsesPerRev = 16;    //        ||
+const float GearRatio = 131.15;   //        ||
+const float Time = 0.01;          //to control the tickers
 
 float limitx =3, limity = 3, positionx = 1, positiony = 1;
 
 int i = 0;                         //counter to go back to initial position if there is no activity
 
 void movex()
-{
+{                            //move motor in the positive x axis
     led1 = 1;
 }
 void movey()
-{
+{                            //move motor in the y axis
     led2 = 1;    
 }
 void movexy()
-{
+{                            //move motor in the x and y axis
     led2 = 1;
     led1 = 1;
 }
 void initialPosition()
-{
+{                             //move the motor back to the initial position
     led2 = 0;
     led1 = 0;
 }
 int choosecase(float emgx, float emgy)
-{
+{                       //decide which case has to be used
     int a = 0;
-    if (emgx > 0 && emgy == 0)
+    if (positionx >= limitx || positiony >= limity)  // limit so that the robot does not break. Maybe try approximating with an elipse.
+    {
+        a = 4;
+        i = 0;
+    }
+    else if (emgx > 0 && emgy == 0)
     {
         a = 1;
         i = 0;
@@ -57,12 +71,7 @@
         a = 3;
         i = 0;
     }
-    else if (positionx >= limitx || positiony >= limity)
-    {
-        a = 4;
-        i = 0;
-    }
-    else if (i > 199)
+    else if (i > 199)          // after some time of inactivity (2s) the motor goes to initial position
     {
         a = 4;
         i = 0;
@@ -71,12 +80,12 @@
     {
         i++;
     }
-    pc.printf("emgx: %f  emgy: %f \r\n",emgx,emgy);
+    //pc.printf("emgx: %f  emgy: %f \r\n",emgx,emgy);
     return a;
 
 }
 void selectcase()
-{
+{                                 //call function to move the motors
     int switchcons = choosecase(emgx,emgy);
     switch(switchcons)
     {