Diff: main.cpp

Revision:

13:787cabccb2be

Parent:

12:5c08ffe8ad1d

Child:

14:e162b5fd0382

--- a/main.cpp	Tue Jun 12 08:08:38 2018 +0000
+++ b/main.cpp	Wed Jun 20 12:24:37 2018 +0000
@@ -6,15 +6,16 @@
 #include <cmath>
 #include "MODSERIAL.h"
 
+
 //Master or Slave? 1=Master, 0=Slave
 static const int identity = 1;
 
 //network config
-static const char* master_ip = "192.168.1.101";
-static const char* slave_ip = "192.168.1.102";
+static const char* master_ip = "192.168.1.101"; //only for direct communication, otherwise set it up using router tables
+static const char* slave_ip = "192.168.1.102";  //only for direct communication, otherwise set it up using router tables
 static const char* MASK = "255.255.255.0";
 static const char* GATEWAY = "192.168.1.1";
-static const char* laptop_IP = "192.168.1.103";
+static const char* laptop_IP = "192.168.1.103"; //only for direct communication, otherwise set it up using router tables
 static const int port = 865;
 
 
@@ -27,7 +28,7 @@
 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
-Endpoint laptop;
+Endpoint laptop;        //Interface to send information for data-storage
 InterruptIn Button1(SW2);
 InterruptIn Button2(SW3);
 Ticker controllerloop;
@@ -55,7 +56,6 @@
 unsigned int check = 1;
 unsigned int counter_not_missed = 0;
 float percentage_received = 0.0;
-
 float input = 0.0;
 
 //measured variables
@@ -110,19 +110,19 @@
 
 
 //Constants
-const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
+const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679; //Why so much accuracy? Because why not?
 const float RadsPerCount = (2 * PI)/(1024*10);
 const float FORCESENSORGAIN = 15.134;
-const int MAX_ENCODER_LEFT = 1333;
-const int MAX_ENCODER_RIGHT = -1453;
+const int MAX_ENCODER_LEFT = 1333; //dependent on setup. PLEASE CHECK
+const int MAX_ENCODER_RIGHT = -1453; //dependent on setup. PLEASE CHECK
 const float WORKSPACEBOUND = PI/6;
 
 const int frequency_pwm = 20000;
 
 //Time delay related variables
 float timedelay = 0.1;//SECONDS
-int delaysteps = timedelay/looptime;
-std::vector<float> delayArrayINPUT(max(delaysteps,1),0.0);
+int delaysteps = timedelay/looptime; 
+std::vector<float> delayArrayINPUT(max(delaysteps,1),0.0); //creating a array that delays the input by x steps, so that time-delay is properly simulated
 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);
@@ -138,7 +138,7 @@
         encoderPos--;
     }
 }
-double velocityFilter(float x)
+float velocityFilter(float x)
 {
     double y = 0.0; //Output
     static double y_1 = 0.0; //Output last loop
@@ -155,7 +155,7 @@
     return (float)y;
 }
 
-void inet_eth()
+void inet_eth() //this function starts the ethernet connection op, including the ipadresses. PLEASE STICK TO STATIC ADDRESSING.
 {
     if(identity==1) {
         eth.init(master_ip, MASK,GATEWAY);
@@ -211,7 +211,7 @@
         x = lower;
 }
 
-void motor_update(float PWM) //angle required to safeguard it from crashing into its stops
+void motor_update(float PWM)//motor function 
 {
     limit(PWM,-1.0f,1.0f);
     if(PWM >= 0.0f) {
@@ -223,16 +223,16 @@
     }
 }
 
-void sensorUpdate()
+void sensorUpdate() 
 {
     angle = encoderPos * RadsPerCount;
     encoder_vel = (encoderPos - prev_encoderPos)/ADDMlooptime; //careful, this function should be called every 1/2500 seconds
     motor_vel = velocityFilter(encoder_vel * RadsPerCount);
     prev_encoderPos = encoderPos;
-    force = -FORCESENSORGAIN*2.0f*(measuredForce - force_offset); //Measured force
+    force = (-FORCESENSORGAIN*2.0f*(measuredForce - force_offset)); //Measured force
 }
 
-void doCalibration()
+void doCalibration() //first state. Move  maximally left, until the stops are hit. 
 {
     u = -0.15;
     motor_update(u);
@@ -240,20 +240,20 @@
     led=1;
     //switching states
     if((abs(motor_vel)<0.001f)&& t.read()>3.0f) {
-        encoderPos = MAX_ENCODER_RIGHT - (1-identity)*180; //to make up for the difference in setups
+        encoderPos = MAX_ENCODER_RIGHT - (1-identity)*180; //to make up for the difference in setups. Make a better way then this. 
         ref_angle = encoderPos * RadsPerCount;
         currentState = stateHoming;
         t.stop();
     }
 }
 
-void doHoming()
+void doHoming() //second state. Move to up. 
 {
     led2=0;
     led=0;
     ref_vel = 0.2;
     if(ref_angle < 0.0f) {
-        ref_angle += ref_vel*ADDMlooptime; //careful, this function should be called every 1/50 seconds
+        ref_angle += ref_vel*ADDMlooptime; 
     } else {
         ref_angle = 0.0;
         ref_vel = 0.0;
@@ -272,7 +272,7 @@
 
 }
 
-void doOperation()
+void doOperation() //final, and infinite state. Do the teleoperation. 
 {
     ref_acc = (force*arm + control_torque- virtualDamping*ref_vel)/virtualInertia;
     ref_vel += ref_acc*ADDMlooptime;
@@ -298,7 +298,7 @@
     //no switching states for now
 }
 
-void loopfunction()
+void loopfunction() //state control
 {
     sensorUpdate();
     switch(currentState) {
@@ -314,21 +314,21 @@
     }
 }
 
-void updateTransparency()
+void updateTransparency() //determining the transparency layer to use. 
 {
     transparancy++;
     if(transparancy>3)
         transparancy = 1;
 }
 
-void updatePassivity()
+void updatePassivity()  //determining wether or not to use the passivity layer 
 {
     passivity++;
     if(passivity>1)
         passivity = 0;
 }
 
-float passivityLayer(float Ftl, float E_in)
+float passivityLayer(float Ftl, float E_in) //just see the report on this topic. Too much stuff to explain.
 {
     tank = tank + E_in - prev_torque*(ref_angle-prev_ref_angle);
     if(tank>0.0f) {
@@ -355,7 +355,7 @@
     }
     if(Ftl>=0.0f) {
         prev_torque = min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
-        return min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc; //min() only takes to arguments, so nested min()'s
+        return min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc; //min() only takes two arguments, so nested min()'s
     } else {
         prev_torque = -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
         return -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
@@ -363,7 +363,7 @@
 
 }
 
-void generateOutput(float variable)
+void generateOutput(float variable) //generate packet to send to counterpart
 {
     memcpy(&output[0],&counter,4);
     memcpy(&output[4],&transparancy,4);
@@ -371,7 +371,7 @@
     memcpy(&output[12],&variable,4);
     memcpy(&output[16],&package_out,4);
 }
-void generateStatus()
+void generateStatus() //generate packet to send to data-storage
 {
     memcpy(&status[0],&counter,4);
     memcpy(&status[4],&ref_angle,4);
@@ -380,7 +380,7 @@
     memcpy(&status[16],&percentage_received,4);
 }
 
-void receiveUDP(void const *argument)
+void receiveUDP(void const *argument) //This is what it is doing, if no trigger is called. 
 {
     while(true) {
         size = socket.receiveFrom(client, data, sizeof(data));
@@ -513,6 +513,9 @@
             controller_check = 0;
             debug = 0;
         }
-        osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
+        osDelay(0.1); // This line is key, otherwise the MBED will not switch to the receive() thread.
     }
-}
\ No newline at end of file
+}
+
+// Also, parts of this code is questionable. As in I don't know why it works. It can be advised to find a library that can have a interrupt on the UPD receive. Or just don't use UDP. although internet. 
+