Diff: main.cpp

Revision:

3:376fccdc7cd6

Parent:

2:c27b0654cffd

Child:

4:610b5051182a

--- a/main.cpp	Tue May 08 08:49:24 2018 +0000
+++ b/main.cpp	Tue May 08 13:14:18 2018 +0000
@@ -1,9 +1,7 @@
 #include "mbed.h"
 #include "EthernetInterface.h"
 #include "rtos.h"
-#include "QEI.h"
 #include <vector>
-#include <cmath>
 
 //Master or Slave? 1=Master, 0=Slave
 static const int identity = 0;
@@ -24,9 +22,15 @@
 UDPSocket socket;       //socket to receive data on
 Endpoint client;        //The virtual other side, not to send actual information to
 Endpoint counterpart;   //The actual other side, this is where the information should go to
+
 Ticker mainloop;
 
-QEI M1(D3,D2,NC,1024,QEI::X4_ENCODING);
+
+InterruptIn EncoderA(D2);
+DigitalIn EncoderB(D3);
+
+DigitalOut M1_DIR(D4);
+PwmOut M1_pwm(D5);
 
 //variables 
 char data[30]= {""};
@@ -38,23 +42,34 @@
 float input = 0.0;
 float angle = 0.0;
 float prev_angle = 0.0;
-float torqueTL = 0.0;
+
+int encoderPos = 0;
 
 bool main_loop_check = 0;
 const float looptime  = 1.0/50; //50Hz
 
-const float Ktl = 20;
-const float Dtl=0.5;
+const float Ktl = 4;
+const float Dtl=0.1;
 
 const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
 const float RadsPerCount = (2 * PI)/(1024*10);
 
-
-int frequency_pwm = 10000;
+int frequency_pwm = 20000;
 
 float timedelay = 0.20; //SECONDS
 std::vector<float> delayArrayINPUT(ceil(timedelay/looptime),0.0);
 
+//FUNCTIONS START HERE
+
+void encoderFunctionA()
+    {
+        if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
+            encoderPos++;
+        } else {
+            encoderPos--;
+        }
+    }
+
 
 void inet_eth(){
     if(identity==1)
@@ -93,13 +108,48 @@
     
 float update_delay(std::vector<float>&array, float new_value)
     {
-        float return_value = array[1];
-        for (int i=0; i<array.size()-1; ++i) 
+    float return_value = array[1];
+    for (int i=0; i<array.size()-1; ++i) 
+    {
+        array[i]=array[i+1];
+    }
+    array.back() = new_value;
+    return return_value;
+    }
+    
+void limit(float &x, float lower, float upper)
+{
+    if (x > upper)
+        x = upper;
+    if (x < lower)
+        x = lower; 
+}
+
+void motor_update(float angle, float &torque) //angle required to safeguard it from crashing into its stops
+    {
+        //if((angle>PI/4)&&(torque>0.0))
+//            {
+//            torque = 0;
+//            }
+//        if((angle<-PI/4)&&(torque<0.0))
+//             {
+//            torque = 0;
+//            }
+        
+        float PWM = torque*0.58; //converting torque to PWM.
+        pc.printf("PWM: %f\r\n",PWM);
+        limit(PWM,-1.0f,1.0f);
+        //pc.printf("PWM: %f\r\n",PWM);
+        if(PWM >= 0.0f)
         {
-            array[i]=array[i+1];
+            M1_DIR = false;
+            M1_pwm = PWM;
+        } else {
+            M1_DIR = true;
+            M1_pwm = -PWM;    
         }
-        array.back() = new_value;
-        return return_value;
+        
+        
     }
 
 void receiveUDP(void const *argument){
@@ -107,6 +157,7 @@
         size = socket.receiveFrom(client, data, sizeof(data));
         if(size > 0){
             data[size] = '\0';
+            
             if(size>5) //first check, an minimum amount of data must have arrived
             {
                 var = strtok(data,"; ");
@@ -115,10 +166,10 @@
                     counter_received = atof(var);
                     var = strtok(NULL,"; ");
                     input = atof(var);
-                    //pc.printf("data: %s \n\r",data);
+                    //pc.printf("input: %f \n\r",input);
+                    //MORE CHECKS NEED TO BE IMPLEMENTED
                 }
             }
-            //pc.printf("data: %s \n\r",data);
             }
         }
     }
@@ -132,20 +183,27 @@
     osThreadCreate(osThread(receiveUDP), NULL);
     led2=1;
     led=1;
+    
     mainloop.attach(&mainlooptrigger,looptime);
     
+    M1_pwm.period(1.0/frequency_pwm);
+    
+    EncoderA.rise(&encoderFunctionA);
+    
     while(true){
-        
         if(main_loop_check==1){
-            angle = M1.getPulses()*RadsPerCount;
+            angle = encoderPos*RadsPerCount;
+            //pc.printf("Angle: %f\r\n",angle);
             float reference = update_delay(delayArrayINPUT,input);
             
-            torqueTL = Ktl*(reference-angle) - Dtl * (angle-prev_angle) / looptime;
+            float torqueTL = Ktl*(reference-angle)- Dtl * (angle-prev_angle) / looptime; 
             
-            pc.printf("Torque: %f\r\n",torqueTL);
+            motor_update(angle,torqueTL);
+            
+            //pc.printf("Torque: %f\r\n",torqueTL);
             sprintf(output, "%i;%f",counter,angle);
             socket.sendTo(counterpart, output, sizeof(output));
-            counter = counter + 1;
+            counter ++;
             led=!led;
             main_loop_check = 0;
             prev_angle = angle;