Diff: main.cpp

Revision:

7:984f363f5e87

Parent:

6:ccbbf4c77d35

Child:

8:5c891d5ebe45

--- a/main.cpp	Tue May 15 11:28:18 2018 +0000
+++ b/main.cpp	Wed May 16 13:53:12 2018 +0000
@@ -6,7 +6,7 @@
 #include <cmath>
 
 //Master or Slave? 1=Master, 0=Slave
-static const int identity = 1;
+static const int identity = 0;
 
 //network config
 static const char* master_ip = "192.168.1.101";
@@ -41,12 +41,12 @@
 double a[3] = {1.00000000000000, -1.561018075, 0.64135153805};
 
 //variables related to networking
-char data[30]= {""};
+char data[25]= {""};
 int size;
-int counter = 1;
-int counter_received = 1;
-char * var;
-char output[30] = {""};
+unsigned int counter = 1;
+unsigned int counter_received = 1;
+unsigned int check = 1;
+char output[25] = {""};
 float input = 0.0;
 
 //measured variables
@@ -70,7 +70,7 @@
 
 //Controller required variables 
 bool controller_check = 0;
-const float looptime  = 1.0/50; //50Hz, for the controller
+const float looptime  = 1.0/50.0; //50Hz, for the controller
 const float ADDMlooptime = 1.0/5000; //5KHz, for the admittance controller 
 enum States {stateCalibration, stateHoming, stateOperation};
 int currentState = stateCalibration;
@@ -112,10 +112,10 @@
 //Time delay related variables
 float timedelay = 0.04; //SECONDS
 int delaysteps = timedelay/looptime;
-std::vector<float> delayArrayINPUT(max(delaysteps,1),0.0);
-std::vector<float> delayArrayMODE(max(delaysteps,1),0.0);
-std::vector<float> delayArrayPASS(max(delaysteps,1),0.0);
-std::vector<float> delayArrayENERGY(max(delaysteps,1),0.0);
+std::vector<float> delayArrayINPUT(max(delaysteps,2),0.0);
+std::vector<float> delayArrayMODE(max(delaysteps,2),0.0);
+std::vector<float> delayArrayPASS(max(delaysteps,2),0.0);
+std::vector<float> delayArrayENERGY(max(delaysteps,2),0.0);
 Timer t;
 
 //FUNCTIONS START HERE
@@ -362,6 +362,17 @@
       
 }
 
+void generateOutput(float variable)
+{
+    memcpy(&output[0],&counter,4);
+    memcpy(&output[4],&transparancy,4);
+    memcpy(&output[8],&passivity,4);
+    memcpy(&output[12],&variable,4);
+    memcpy(&output[16],&package_out,4);
+    //pc.printf("output:%i,%i,%i,%f,%f\r\n",counter,transparancy,passivity,variable,package_out);
+    
+}
+
 void receiveUDP(void const *argument){
     while(true)
     {
@@ -369,21 +380,17 @@
         if(size > 0)
         {
             data[size] = '\0';
-            pc.printf("data:%s\r\n",data);
-            if(size>5) //first check, an minimum amount of data must have arrived
+            if(size>18) //first check, an minimum amount of data must have arrived
                 {
-                var = strtok(data,"; ");
-                if(counter_received < atof(var)) //second check, data must be newer
+                memcpy(&check,&data[0],4);
+                if(counter_received < check) //second check, data must be newer
                     {
-                    counter_received = atof(var);
-                    var = strtok(NULL,"; ");
-                    received_transparancy = atof(var);
-                    var = strtok(NULL,"; ");
-                    received_passivity = atof(var);
-                    var = strtok(NULL,"; ");
-                    input = atof(var);
-                    var = strtok(NULL,"; ");
-                    received_package += atof(var);
+                    counter_received=check;
+                    memcpy(&received_transparancy,&data[4],4);
+                    memcpy(&received_passivity,&data[8],4);
+                    memcpy(&input,&data[12],4);
+                    memcpy(&received_package,&data[16],4);    
+                    //pc.printf("data:%i,%i,%i,%f,%f\r\n",counter_received,received_transparancy,received_passivity,input,received_package);
                     }
             }
         }
@@ -412,9 +419,9 @@
     
     M1_pwm.period(1.0/frequency_pwm);
     EncoderA.rise(&encoderFunctionA);
-    
-    Button1.fall(&updateTransparency);
-    Button2.fall(&updatePassivity);
+        
+    Button1.rise(&updateTransparency);
+    Button2.rise(&updatePassivity);
     
     t.start();
     
@@ -438,7 +445,7 @@
                         control_torque = passivityLayer(torque_tlc,package_in);
                     else
                         control_torque = torque_tlc;
-                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);                   
+                    generateOutput(ref_angle);                  
                 }
                 else if(transparancy==2)
                 {
@@ -446,8 +453,8 @@
                     if(current_passivity==1)
                         control_torque = passivityLayer(torque_tlc,package_in);
                     else
-                        control_torque = torque_tlc;                 
-                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,force,package_out); 
+                        control_torque = torque_tlc;
+                    generateOutput(force);                 
                 }
                 else if(transparancy==3)
                 {
@@ -455,8 +462,8 @@
                     if(current_passivity==1)
                         control_torque = passivityLayer(torque_tlc,package_in);
                     else
-                        control_torque = torque_tlc;                  
-                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
+                        control_torque = torque_tlc;
+                    generateOutput(ref_angle);                  
                 }   
             }
             else if(identity == 1)
@@ -467,8 +474,8 @@
                     if(current_passivity==1)
                         control_torque = passivityLayer(torque_tlc,package_in);
                     else
-                        control_torque = torque_tlc;                    
-                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
+                        control_torque = torque_tlc;
+                    generateOutput(ref_angle);                   
                 }
                 else if(transparancy==2)
                 {
@@ -476,8 +483,8 @@
                     if(current_passivity==1)
                         control_torque = passivityLayer(torque_tlc,package_in);
                     else
-                        control_torque = torque_tlc;                   
-                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
+                        control_torque = torque_tlc;    
+                    generateOutput(ref_angle);                                       
                 }
                 else if(transparancy==3)
                 {
@@ -485,11 +492,12 @@
                     if(current_passivity==1)
                         control_torque = passivityLayer(torque_tlc,package_in);
                     else
-                        control_torque = torque_tlc;                    
-                    sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,force,package_out);
+                        control_torque = torque_tlc;   
+                    generateOutput(force);                                         
                 }
             }
-            
+            //pc.printf("output: %s\r\n",output);
+            //pc.printf("received input: %i\r\n",received_input);
             socket.sendTo(counterpart, output, sizeof(output));
             counter ++;
             led=!led;