Bas Koster / Mbed 2 deprecated Teleoperations

Bachelor Assignment for delayed teleoperating systems

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp@6:ccbbf4c77d35, 2018-05-15 (annotated)

Committer:
darth_bachious
Date:
Tue May 15 11:28:18 2018 +0000
Revision:
6:ccbbf4c77d35
Parent:
5:4d5b077b3fe6
Child:
7:984f363f5e87
Working switch functionality between systems, passivityfunction NOT working;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
darth_bachious 0:2f89dec3e2ab 1 #include "mbed.h"
darth_bachious 0:2f89dec3e2ab 2 #include "EthernetInterface.h"
darth_bachious 0:2f89dec3e2ab 3 #include "rtos.h"
darth_bachious 2:c27b0654cffd 4 #include <vector>
darth_bachious 5:4d5b077b3fe6 5 #include "FastPWM.h"
darth_bachious 6:ccbbf4c77d35 6 #include <cmath>
darth_bachious 0:2f89dec3e2ab 7
darth_bachious 1:853939e38acd 8 //Master or Slave? 1=Master, 0=Slave
darth_bachious 6:ccbbf4c77d35 9 static const int identity = 1;
darth_bachious 0:2f89dec3e2ab 10
darth_bachious 0:2f89dec3e2ab 11 //network config
darth_bachious 1:853939e38acd 12 static const char* master_ip = "192.168.1.101";
darth_bachious 1:853939e38acd 13 static const char* slave_ip = "192.168.1.102";
darth_bachious 1:853939e38acd 14 static const char* MASK = "255.255.255.0";
darth_bachious 1:853939e38acd 15 static const char* GATEWAY = "192.168.1.1";
darth_bachious 0:2f89dec3e2ab 16 static const int port = 865;
darth_bachious 0:2f89dec3e2ab 17
darth_bachious 0:2f89dec3e2ab 18 //declaration of interfaces
darth_bachious 0:2f89dec3e2ab 19 DigitalOut led(LED_GREEN);
darth_bachious 0:2f89dec3e2ab 20 DigitalOut led2(LED_RED);
darth_bachious 0:2f89dec3e2ab 21 EthernetInterface eth; //network
darth_bachious 0:2f89dec3e2ab 22 Serial pc(USBTX, USBRX);//create PC interface
darth_bachious 0:2f89dec3e2ab 23 UDPSocket socket; //socket to receive data on
darth_bachious 0:2f89dec3e2ab 24 Endpoint client; //The virtual other side, not to send actual information to
darth_bachious 0:2f89dec3e2ab 25 Endpoint counterpart; //The actual other side, this is where the information should go to
darth_bachious 6:ccbbf4c77d35 26 InterruptIn Button1(SW2);
darth_bachious 6:ccbbf4c77d35 27 InterruptIn Button2(SW3);
darth_bachious 5:4d5b077b3fe6 28 Ticker controllerloop;
darth_bachious 0:2f89dec3e2ab 29 Ticker mainloop;
darth_bachious 6:ccbbf4c77d35 30 DigitalOut debug(D11);
darth_bachious 0:2f89dec3e2ab 31
darth_bachious 4:610b5051182a 32 //Motor interfaces and variables
darth_bachious 3:376fccdc7cd6 33 InterruptIn EncoderA(D2);
darth_bachious 6:ccbbf4c77d35 34 DigitalIn EncoderB(D3);
darth_bachious 3:376fccdc7cd6 35 DigitalOut M1_DIR(D4);
darth_bachious 5:4d5b077b3fe6 36 FastPWM M1_pwm(D5);
darth_bachious 4:610b5051182a 37 AnalogIn measuredForce(A5);
darth_bachious 4:610b5051182a 38
darth_bachious 4:610b5051182a 39 //Low pass filter filter coeffs, 2nd order, 50 Hz
darth_bachious 4:610b5051182a 40 double b[3] = {0.020083365564211, 0.020083365564211*2, 0.020083365564211};
darth_bachious 4:610b5051182a 41 double a[3] = {1.00000000000000, -1.561018075, 0.64135153805};
darth_bachious 4:610b5051182a 42
darth_bachious 4:610b5051182a 43 //variables related to networking
darth_bachious 1:853939e38acd 44 char data[30]= {""};
darth_bachious 0:2f89dec3e2ab 45 int size;
darth_bachious 1:853939e38acd 46 int counter = 1;
darth_bachious 1:853939e38acd 47 int counter_received = 1;
darth_bachious 0:2f89dec3e2ab 48 char * var;
darth_bachious 1:853939e38acd 49 char output[30] = {""};
darth_bachious 2:c27b0654cffd 50 float input = 0.0;
darth_bachious 4:610b5051182a 51
darth_bachious 4:610b5051182a 52 //measured variables
darth_bachious 2:c27b0654cffd 53 float angle = 0.0;
darth_bachious 5:4d5b077b3fe6 54 int encoderPos = 0;
darth_bachious 5:4d5b077b3fe6 55 int prev_encoderPos = 0;
darth_bachious 4:610b5051182a 56 float encoder_vel = 0.0;
darth_bachious 4:610b5051182a 57 float motor_vel = 0.0;
darth_bachious 4:610b5051182a 58 float force = 0.0;
darth_bachious 5:4d5b077b3fe6 59 float control_torque = 0.0;
darth_bachious 6:ccbbf4c77d35 60 float force_offset = 0.5;
darth_bachious 3:376fccdc7cd6 61
darth_bachious 4:610b5051182a 62 //Reference, used in admittance
darth_bachious 4:610b5051182a 63 float ref_angle = 0.0;
darth_bachious 4:610b5051182a 64 float ref_vel = 0.0;
darth_bachious 4:610b5051182a 65 float ref_acc = 0.0;
darth_bachious 6:ccbbf4c77d35 66 const float virtualMass = 0.005;
darth_bachious 6:ccbbf4c77d35 67 const float virtualDamping = 0.05;
darth_bachious 4:610b5051182a 68 const float virtualStiffness = 5;
darth_bachious 6:ccbbf4c77d35 69 const float arm = 0.07;
darth_bachious 4:610b5051182a 70
darth_bachious 6:ccbbf4c77d35 71 //Controller required variables
darth_bachious 5:4d5b077b3fe6 72 bool controller_check = 0;
darth_bachious 5:4d5b077b3fe6 73 const float looptime = 1.0/50; //50Hz, for the controller
darth_bachious 5:4d5b077b3fe6 74 const float ADDMlooptime = 1.0/5000; //5KHz, for the admittance controller
darth_bachious 6:ccbbf4c77d35 75 enum States {stateCalibration, stateHoming, stateOperation};
darth_bachious 4:610b5051182a 76 int currentState = stateCalibration;
darth_bachious 6:ccbbf4c77d35 77 int transparancy = 1; // 1=Pos-Pos, 2=Pos-Force, 3=Force-Pos
darth_bachious 6:ccbbf4c77d35 78 int passivity =0; // 0=off, 1=on
darth_bachious 6:ccbbf4c77d35 79 int received_transparancy = 1;
darth_bachious 6:ccbbf4c77d35 80 int received_passivity = 0;
darth_bachious 4:610b5051182a 81
darth_bachious 4:610b5051182a 82 //Controller parameters
darth_bachious 5:4d5b077b3fe6 83 const float Ktl = 1; //high level controller parameters
darth_bachious 3:376fccdc7cd6 84 const float Dtl=0.1;
darth_bachious 4:610b5051182a 85 const float Kp = 5; //low level controller parameters
darth_bachious 4:610b5051182a 86 const float Dp = 0.05;
darth_bachious 6:ccbbf4c77d35 87 float u = 0.0; //serves as input variable for the motor;
darth_bachious 4:610b5051182a 88
darth_bachious 6:ccbbf4c77d35 89 //passivity layer constant
darth_bachious 6:ccbbf4c77d35 90 const float beta = 0.15;
darth_bachious 6:ccbbf4c77d35 91 const float Hd = 0.5;
darth_bachious 6:ccbbf4c77d35 92 const float alpha = 10;
darth_bachious 6:ccbbf4c77d35 93
darth_bachious 6:ccbbf4c77d35 94 //passivity layer variables
darth_bachious 6:ccbbf4c77d35 95 float tank = 0.0;
darth_bachious 6:ccbbf4c77d35 96 float prev_ref_angle = 0.0;
darth_bachious 6:ccbbf4c77d35 97 float package_out = 0.0;
darth_bachious 6:ccbbf4c77d35 98 float received_package = 0.0;
darth_bachious 6:ccbbf4c77d35 99 float prev_torque = 0.0;
darth_bachious 6:ccbbf4c77d35 100
darth_bachious 4:610b5051182a 101
darth_bachious 4:610b5051182a 102 //Constants
darth_bachious 0:2f89dec3e2ab 103 const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
darth_bachious 2:c27b0654cffd 104 const float RadsPerCount = (2 * PI)/(1024*10);
darth_bachious 4:610b5051182a 105 const float FORCESENSORGAIN = 15.134;
darth_bachious 4:610b5051182a 106 const int MAX_ENCODER_LEFT = 1333;
darth_bachious 4:610b5051182a 107 const int MAX_ENCODER_RIGHT = -1453;
darth_bachious 5:4d5b077b3fe6 108 const float WORKSPACEBOUND = PI/6;
darth_bachious 2:c27b0654cffd 109
darth_bachious 6:ccbbf4c77d35 110 const int frequency_pwm = 20000;
darth_bachious 0:2f89dec3e2ab 111
darth_bachious 4:610b5051182a 112 //Time delay related variables
darth_bachious 4:610b5051182a 113 float timedelay = 0.04; //SECONDS
darth_bachious 6:ccbbf4c77d35 114 int delaysteps = timedelay/looptime;
darth_bachious 6:ccbbf4c77d35 115 std::vector<float> delayArrayINPUT(max(delaysteps,1),0.0);
darth_bachious 6:ccbbf4c77d35 116 std::vector<float> delayArrayMODE(max(delaysteps,1),0.0);
darth_bachious 6:ccbbf4c77d35 117 std::vector<float> delayArrayPASS(max(delaysteps,1),0.0);
darth_bachious 6:ccbbf4c77d35 118 std::vector<float> delayArrayENERGY(max(delaysteps,1),0.0);
darth_bachious 4:610b5051182a 119 Timer t;
darth_bachious 4:610b5051182a 120
darth_bachious 3:376fccdc7cd6 121 //FUNCTIONS START HERE
darth_bachious 3:376fccdc7cd6 122
darth_bachious 3:376fccdc7cd6 123 void encoderFunctionA()
darth_bachious 3:376fccdc7cd6 124 {
darth_bachious 3:376fccdc7cd6 125 if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
darth_bachious 3:376fccdc7cd6 126 encoderPos++;
darth_bachious 3:376fccdc7cd6 127 } else {
darth_bachious 3:376fccdc7cd6 128 encoderPos--;
darth_bachious 3:376fccdc7cd6 129 }
darth_bachious 3:376fccdc7cd6 130 }
darth_bachious 4:610b5051182a 131 double velocityFilter(float x)
darth_bachious 4:610b5051182a 132 {
darth_bachious 4:610b5051182a 133 double y = 0.0; //Output
darth_bachious 4:610b5051182a 134 static double y_1 = 0.0; //Output last loop
darth_bachious 4:610b5051182a 135 static double y_2 = 0.0; //Output 2 loops ago
darth_bachious 4:610b5051182a 136 static double x_1 = 0.0; //Input last loop
darth_bachious 4:610b5051182a 137 static double x_2 = 0.0; //Input two loops ago
darth_bachious 3:376fccdc7cd6 138
darth_bachious 4:610b5051182a 139 //Finite difference equation for 2nd order Butterworth low-pass
darth_bachious 4:610b5051182a 140 y = -a[1]*y_1 - a[2]*y_2 + x*b[0] + x_1*b[1] + x_2*b[2];
darth_bachious 4:610b5051182a 141 y_2 = y_1;
darth_bachious 4:610b5051182a 142 y_1 = y;
darth_bachious 4:610b5051182a 143 x_2 = x_1;
darth_bachious 4:610b5051182a 144 x_1 = x;
darth_bachious 4:610b5051182a 145 return (float)y;
darth_bachious 4:610b5051182a 146 }
darth_bachious 0:2f89dec3e2ab 147
darth_bachious 0:2f89dec3e2ab 148 void inet_eth(){
darth_bachious 1:853939e38acd 149 if(identity==1)
darth_bachious 1:853939e38acd 150 {
darth_bachious 1:853939e38acd 151 eth.init(master_ip, MASK,GATEWAY);
darth_bachious 1:853939e38acd 152 eth.connect();
darth_bachious 0:2f89dec3e2ab 153
darth_bachious 1:853939e38acd 154 socket.bind(port);
darth_bachious 1:853939e38acd 155 counterpart.set_address(slave_ip,port);
darth_bachious 1:853939e38acd 156 }
darth_bachious 1:853939e38acd 157 else if(identity==0)
darth_bachious 1:853939e38acd 158 {
darth_bachious 1:853939e38acd 159 eth.init(slave_ip, MASK,GATEWAY);
darth_bachious 1:853939e38acd 160 eth.connect();
darth_bachious 1:853939e38acd 161
darth_bachious 1:853939e38acd 162 socket.bind(port);
darth_bachious 1:853939e38acd 163 counterpart.set_address(master_ip,port);
darth_bachious 1:853939e38acd 164 }
darth_bachious 1:853939e38acd 165
darth_bachious 0:2f89dec3e2ab 166 }
darth_bachious 0:2f89dec3e2ab 167
darth_bachious 0:2f89dec3e2ab 168 void inet_USB(){
darth_bachious 2:c27b0654cffd 169 pc.baud(115200);
darth_bachious 0:2f89dec3e2ab 170 }
darth_bachious 0:2f89dec3e2ab 171
darth_bachious 0:2f89dec3e2ab 172
darth_bachious 0:2f89dec3e2ab 173 void end_eth(){
darth_bachious 0:2f89dec3e2ab 174 socket.close();
darth_bachious 0:2f89dec3e2ab 175 eth.disconnect();
darth_bachious 0:2f89dec3e2ab 176 }
darth_bachious 0:2f89dec3e2ab 177
darth_bachious 5:4d5b077b3fe6 178 void controllertrigger()
darth_bachious 0:2f89dec3e2ab 179 {
darth_bachious 5:4d5b077b3fe6 180 controller_check = 1;
darth_bachious 0:2f89dec3e2ab 181 }
darth_bachious 2:c27b0654cffd 182
darth_bachious 4:610b5051182a 183
darth_bachious 2:c27b0654cffd 184 float update_delay(std::vector<float>&array, float new_value)
darth_bachious 2:c27b0654cffd 185 {
darth_bachious 3:376fccdc7cd6 186 float return_value = array[1];
darth_bachious 3:376fccdc7cd6 187 for (int i=0; i<array.size()-1; ++i)
darth_bachious 3:376fccdc7cd6 188 {
darth_bachious 3:376fccdc7cd6 189 array[i]=array[i+1];
darth_bachious 3:376fccdc7cd6 190 }
darth_bachious 3:376fccdc7cd6 191 array.back() = new_value;
darth_bachious 3:376fccdc7cd6 192 return return_value;
darth_bachious 3:376fccdc7cd6 193 }
darth_bachious 3:376fccdc7cd6 194
darth_bachious 3:376fccdc7cd6 195 void limit(float &x, float lower, float upper)
darth_bachious 3:376fccdc7cd6 196 {
darth_bachious 3:376fccdc7cd6 197 if (x > upper)
darth_bachious 3:376fccdc7cd6 198 x = upper;
darth_bachious 3:376fccdc7cd6 199 if (x < lower)
darth_bachious 3:376fccdc7cd6 200 x = lower;
darth_bachious 3:376fccdc7cd6 201 }
darth_bachious 3:376fccdc7cd6 202
darth_bachious 4:610b5051182a 203 void motor_update(float PWM) //angle required to safeguard it from crashing into its stops
darth_bachious 4:610b5051182a 204 {
darth_bachious 4:610b5051182a 205 limit(PWM,-1.0f,1.0f);
darth_bachious 4:610b5051182a 206 if(PWM >= 0.0f)
darth_bachious 4:610b5051182a 207 {
darth_bachious 4:610b5051182a 208 M1_DIR = false;
darth_bachious 4:610b5051182a 209 M1_pwm = PWM;
darth_bachious 4:610b5051182a 210 } else {
darth_bachious 4:610b5051182a 211 M1_DIR = true;
darth_bachious 4:610b5051182a 212 M1_pwm = -PWM;
darth_bachious 4:610b5051182a 213 }
darth_bachious 4:610b5051182a 214 }
darth_bachious 4:610b5051182a 215
darth_bachious 4:610b5051182a 216 void sensorUpdate()
darth_bachious 4:610b5051182a 217 {
darth_bachious 4:610b5051182a 218 angle = encoderPos * RadsPerCount;
darth_bachious 5:4d5b077b3fe6 219 encoder_vel = (encoderPos - prev_encoderPos)/ADDMlooptime; //careful, this function should be called every 1/5000 seconds
darth_bachious 4:610b5051182a 220 motor_vel = velocityFilter(encoder_vel * RadsPerCount);
darth_bachious 4:610b5051182a 221 prev_encoderPos = encoderPos;
darth_bachious 6:ccbbf4c77d35 222 force = -FORCESENSORGAIN*2.0f*(measuredForce - force_offset); //Measured force
darth_bachious 4:610b5051182a 223 }
darth_bachious 4:610b5051182a 224
darth_bachious 4:610b5051182a 225 void doCalibration()
darth_bachious 4:610b5051182a 226 {
darth_bachious 4:610b5051182a 227 u = 0.09;
darth_bachious 4:610b5051182a 228 motor_update(u);
darth_bachious 6:ccbbf4c77d35 229 led2=0;
darth_bachious 6:ccbbf4c77d35 230 led=1;
darth_bachious 4:610b5051182a 231 //switching states
darth_bachious 6:ccbbf4c77d35 232 if((abs(motor_vel)<0.001f)&& t.read()>3.0f)
darth_bachious 3:376fccdc7cd6 233 {
darth_bachious 4:610b5051182a 234 encoderPos = MAX_ENCODER_LEFT;
darth_bachious 4:610b5051182a 235 ref_angle = encoderPos * RadsPerCount;
darth_bachious 4:610b5051182a 236 currentState = stateHoming;
darth_bachious 4:610b5051182a 237 t.stop();
darth_bachious 4:610b5051182a 238 }
darth_bachious 4:610b5051182a 239 }
darth_bachious 4:610b5051182a 240
darth_bachious 4:610b5051182a 241 void doHoming()
darth_bachious 4:610b5051182a 242 {
darth_bachious 4:610b5051182a 243 led2=0;
darth_bachious 4:610b5051182a 244 led=0;
darth_bachious 4:610b5051182a 245 ref_vel = -0.2;
darth_bachious 6:ccbbf4c77d35 246 if(ref_angle > 0.0f)
darth_bachious 4:610b5051182a 247 {
darth_bachious 5:4d5b077b3fe6 248 ref_angle += ref_vel*ADDMlooptime; //careful, this function should be called every 1/50 seconds
darth_bachious 4:610b5051182a 249 }
darth_bachious 4:610b5051182a 250 else
darth_bachious 4:610b5051182a 251 {
darth_bachious 4:610b5051182a 252 ref_angle = 0.0;
darth_bachious 4:610b5051182a 253 ref_vel = 0.0;
darth_bachious 4:610b5051182a 254 }
darth_bachious 4:610b5051182a 255 u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);
darth_bachious 4:610b5051182a 256 motor_update(u);
darth_bachious 4:610b5051182a 257
darth_bachious 4:610b5051182a 258 //switching states
darth_bachious 6:ccbbf4c77d35 259 if ((abs(encoderPos)<20)&&abs((ref_vel - motor_vel)<0.02f))
darth_bachious 4:610b5051182a 260 {
darth_bachious 4:610b5051182a 261 currentState = stateOperation;
darth_bachious 6:ccbbf4c77d35 262 force_offset = measuredForce;
darth_bachious 4:610b5051182a 263 motor_update(0.0);
darth_bachious 5:4d5b077b3fe6 264 led2=1;
darth_bachious 6:ccbbf4c77d35 265 led=1;
darth_bachious 4:610b5051182a 266 }
darth_bachious 3:376fccdc7cd6 267
darth_bachious 4:610b5051182a 268 }
darth_bachious 4:610b5051182a 269
darth_bachious 4:610b5051182a 270 void doOperation()
darth_bachious 4:610b5051182a 271 {
darth_bachious 5:4d5b077b3fe6 272 ref_acc = (force*arm + control_torque- virtualDamping*ref_vel)/virtualMass;
darth_bachious 5:4d5b077b3fe6 273 ref_vel += ref_acc*ADDMlooptime;
darth_bachious 5:4d5b077b3fe6 274 ref_angle += ref_vel*ADDMlooptime;
darth_bachious 5:4d5b077b3fe6 275 if(ref_angle > WORKSPACEBOUND)
darth_bachious 5:4d5b077b3fe6 276 {
darth_bachious 5:4d5b077b3fe6 277 ref_vel = 0.0;
darth_bachious 5:4d5b077b3fe6 278 ref_acc = 0.0;
darth_bachious 5:4d5b077b3fe6 279 ref_angle = WORKSPACEBOUND;
darth_bachious 5:4d5b077b3fe6 280 } else if(ref_angle < -WORKSPACEBOUND)
darth_bachious 5:4d5b077b3fe6 281 {
darth_bachious 5:4d5b077b3fe6 282 ref_vel = 0.0;
darth_bachious 5:4d5b077b3fe6 283 ref_acc = 0.0;
darth_bachious 5:4d5b077b3fe6 284 ref_angle = -WORKSPACEBOUND;
darth_bachious 5:4d5b077b3fe6 285 }
darth_bachious 5:4d5b077b3fe6 286 u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);
darth_bachious 4:610b5051182a 287 motor_update(u);
darth_bachious 6:ccbbf4c77d35 288
darth_bachious 6:ccbbf4c77d35 289 //no switching states for now
darth_bachious 5:4d5b077b3fe6 290 }
darth_bachious 5:4d5b077b3fe6 291
darth_bachious 5:4d5b077b3fe6 292 void loopfunction()
darth_bachious 5:4d5b077b3fe6 293 {
darth_bachious 5:4d5b077b3fe6 294 sensorUpdate();
darth_bachious 5:4d5b077b3fe6 295 switch(currentState)
darth_bachious 5:4d5b077b3fe6 296 {
darth_bachious 5:4d5b077b3fe6 297 case stateCalibration:
darth_bachious 5:4d5b077b3fe6 298 doCalibration();
darth_bachious 5:4d5b077b3fe6 299 break;
darth_bachious 5:4d5b077b3fe6 300 case stateHoming:
darth_bachious 5:4d5b077b3fe6 301 doHoming();
darth_bachious 5:4d5b077b3fe6 302 break;
darth_bachious 5:4d5b077b3fe6 303 case stateOperation:
darth_bachious 5:4d5b077b3fe6 304 doOperation();
darth_bachious 5:4d5b077b3fe6 305 break;
darth_bachious 5:4d5b077b3fe6 306 }
darth_bachious 4:610b5051182a 307 }
darth_bachious 0:2f89dec3e2ab 308
darth_bachious 6:ccbbf4c77d35 309 void updateTransparency()
darth_bachious 6:ccbbf4c77d35 310 {
darth_bachious 6:ccbbf4c77d35 311 transparancy++;
darth_bachious 6:ccbbf4c77d35 312 if(transparancy>3)
darth_bachious 6:ccbbf4c77d35 313 transparancy = 1;
darth_bachious 6:ccbbf4c77d35 314 }
darth_bachious 6:ccbbf4c77d35 315
darth_bachious 6:ccbbf4c77d35 316 void updatePassivity()
darth_bachious 6:ccbbf4c77d35 317 {
darth_bachious 6:ccbbf4c77d35 318 passivity++;
darth_bachious 6:ccbbf4c77d35 319 if(passivity>1)
darth_bachious 6:ccbbf4c77d35 320 passivity = 0;
darth_bachious 6:ccbbf4c77d35 321 }
darth_bachious 6:ccbbf4c77d35 322
darth_bachious 6:ccbbf4c77d35 323 float passivityLayer(float Ftl, float E_in)
darth_bachious 6:ccbbf4c77d35 324 {
darth_bachious 6:ccbbf4c77d35 325 tank = tank + E_in - prev_torque*(ref_angle-prev_ref_angle);
darth_bachious 6:ccbbf4c77d35 326 if(tank>0.0f)
darth_bachious 6:ccbbf4c77d35 327 {
darth_bachious 6:ccbbf4c77d35 328 package_out = tank*beta;
darth_bachious 6:ccbbf4c77d35 329 tank = tank - package_out;
darth_bachious 6:ccbbf4c77d35 330 } else
darth_bachious 6:ccbbf4c77d35 331 package_out = 0.0;
darth_bachious 6:ccbbf4c77d35 332
darth_bachious 6:ccbbf4c77d35 333 float FMAX1 = 0.0;
darth_bachious 6:ccbbf4c77d35 334 if(tank>0.0f)
darth_bachious 6:ccbbf4c77d35 335 {
darth_bachious 6:ccbbf4c77d35 336 float FMAX1 = abs(Ftl);
darth_bachious 6:ccbbf4c77d35 337 }
darth_bachious 6:ccbbf4c77d35 338 float FMAX2 = abs(tank/(ref_vel*looptime));
darth_bachious 6:ccbbf4c77d35 339
darth_bachious 6:ccbbf4c77d35 340 float FMAX3 = 0.15;
darth_bachious 6:ccbbf4c77d35 341
darth_bachious 6:ccbbf4c77d35 342 prev_ref_angle = ref_angle;
darth_bachious 6:ccbbf4c77d35 343
darth_bachious 6:ccbbf4c77d35 344 float Ftlc = 0.0;
darth_bachious 6:ccbbf4c77d35 345
darth_bachious 6:ccbbf4c77d35 346 if((tank<Hd)&&(identity==1))
darth_bachious 6:ccbbf4c77d35 347 {
darth_bachious 6:ccbbf4c77d35 348 Ftlc = - alpha*(Hd-tank)*ref_vel;
darth_bachious 6:ccbbf4c77d35 349 }
darth_bachious 6:ccbbf4c77d35 350 //pc.printf("Ftlc: %f\r\n",Ftlc);
darth_bachious 6:ccbbf4c77d35 351 //pc.printf("tank: %f\r\n",tank);
darth_bachious 6:ccbbf4c77d35 352 if(Ftl>=0.0f)
darth_bachious 6:ccbbf4c77d35 353 {
darth_bachious 6:ccbbf4c77d35 354 prev_torque = min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
darth_bachious 6:ccbbf4c77d35 355 return min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc; //min() only takes to arguments, so nested min()'s
darth_bachious 6:ccbbf4c77d35 356 }
darth_bachious 6:ccbbf4c77d35 357 else
darth_bachious 6:ccbbf4c77d35 358 {
darth_bachious 6:ccbbf4c77d35 359 prev_torque = -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))-Ftlc;
darth_bachious 6:ccbbf4c77d35 360 return -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))-Ftlc;
darth_bachious 6:ccbbf4c77d35 361 }
darth_bachious 6:ccbbf4c77d35 362
darth_bachious 6:ccbbf4c77d35 363 }
darth_bachious 6:ccbbf4c77d35 364
darth_bachious 0:2f89dec3e2ab 365 void receiveUDP(void const *argument){
darth_bachious 4:610b5051182a 366 while(true)
darth_bachious 4:610b5051182a 367 {
darth_bachious 0:2f89dec3e2ab 368 size = socket.receiveFrom(client, data, sizeof(data));
darth_bachious 4:610b5051182a 369 if(size > 0)
darth_bachious 4:610b5051182a 370 {
darth_bachious 0:2f89dec3e2ab 371 data[size] = '\0';
darth_bachious 6:ccbbf4c77d35 372 pc.printf("data:%s\r\n",data);
darth_bachious 2:c27b0654cffd 373 if(size>5) //first check, an minimum amount of data must have arrived
darth_bachious 4:610b5051182a 374 {
darth_bachious 2:c27b0654cffd 375 var = strtok(data,"; ");
darth_bachious 2:c27b0654cffd 376 if(counter_received < atof(var)) //second check, data must be newer
darth_bachious 4:610b5051182a 377 {
darth_bachious 2:c27b0654cffd 378 counter_received = atof(var);
darth_bachious 2:c27b0654cffd 379 var = strtok(NULL,"; ");
darth_bachious 6:ccbbf4c77d35 380 received_transparancy = atof(var);
darth_bachious 6:ccbbf4c77d35 381 var = strtok(NULL,"; ");
darth_bachious 6:ccbbf4c77d35 382 received_passivity = atof(var);
darth_bachious 6:ccbbf4c77d35 383 var = strtok(NULL,"; ");
darth_bachious 2:c27b0654cffd 384 input = atof(var);
darth_bachious 6:ccbbf4c77d35 385 var = strtok(NULL,"; ");
darth_bachious 6:ccbbf4c77d35 386 received_package += atof(var);
darth_bachious 4:610b5051182a 387 }
darth_bachious 0:2f89dec3e2ab 388 }
darth_bachious 0:2f89dec3e2ab 389 }
darth_bachious 0:2f89dec3e2ab 390 }
darth_bachious 4:610b5051182a 391 }
darth_bachious 0:2f89dec3e2ab 392
darth_bachious 0:2f89dec3e2ab 393 osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);
darth_bachious 0:2f89dec3e2ab 394
darth_bachious 0:2f89dec3e2ab 395 int main(){
darth_bachious 1:853939e38acd 396 inet_eth();
darth_bachious 0:2f89dec3e2ab 397 inet_USB();
darth_bachious 0:2f89dec3e2ab 398
darth_bachious 0:2f89dec3e2ab 399 osThreadCreate(osThread(receiveUDP), NULL);
darth_bachious 0:2f89dec3e2ab 400 led2=1;
darth_bachious 0:2f89dec3e2ab 401 led=1;
darth_bachious 5:4d5b077b3fe6 402
darth_bachious 6:ccbbf4c77d35 403 //Set all interrupt requests to 2nd place priority
darth_bachious 5:4d5b077b3fe6 404 for(int n = 0; n < 86; n++) {
darth_bachious 5:4d5b077b3fe6 405 NVIC_SetPriority((IRQn)n,1);
darth_bachious 5:4d5b077b3fe6 406 }
darth_bachious 5:4d5b077b3fe6 407 //Set out motor encoder interrupt to 1st place priority
darth_bachious 5:4d5b077b3fe6 408 NVIC_SetPriority(PORTB_IRQn,0);
darth_bachious 6:ccbbf4c77d35 409
darth_bachious 5:4d5b077b3fe6 410 controllerloop.attach(&controllertrigger,looptime);
darth_bachious 5:4d5b077b3fe6 411 mainloop.attach(&loopfunction,ADDMlooptime);
darth_bachious 5:4d5b077b3fe6 412
darth_bachious 4:610b5051182a 413 M1_pwm.period(1.0/frequency_pwm);
darth_bachious 4:610b5051182a 414 EncoderA.rise(&encoderFunctionA);
darth_bachious 0:2f89dec3e2ab 415
darth_bachious 6:ccbbf4c77d35 416 Button1.fall(&updateTransparency);
darth_bachious 6:ccbbf4c77d35 417 Button2.fall(&updatePassivity);
darth_bachious 6:ccbbf4c77d35 418
darth_bachious 4:610b5051182a 419 t.start();
darth_bachious 3:376fccdc7cd6 420
darth_bachious 0:2f89dec3e2ab 421 while(true){
darth_bachious 5:4d5b077b3fe6 422 if(controller_check==1){
darth_bachious 6:ccbbf4c77d35 423 debug = 1;
darth_bachious 6:ccbbf4c77d35 424 float received_input = update_delay(delayArrayINPUT,input);
darth_bachious 6:ccbbf4c77d35 425 float current_transparancy = update_delay(delayArrayMODE,received_transparancy);
darth_bachious 6:ccbbf4c77d35 426 float current_passivity = update_delay(delayArrayPASS,received_passivity);
darth_bachious 6:ccbbf4c77d35 427 float package_in = update_delay(delayArrayENERGY,received_package);
darth_bachious 6:ccbbf4c77d35 428 received_package = 0; //IMPORTANT, WILL EXPLODE OTHERWISE
darth_bachious 3:376fccdc7cd6 429
darth_bachious 6:ccbbf4c77d35 430 if(identity==0)
darth_bachious 6:ccbbf4c77d35 431 {
darth_bachious 6:ccbbf4c77d35 432 transparancy = current_transparancy;
darth_bachious 6:ccbbf4c77d35 433 passivity = current_passivity;
darth_bachious 6:ccbbf4c77d35 434 if(transparancy==1)
darth_bachious 6:ccbbf4c77d35 435 {
darth_bachious 6:ccbbf4c77d35 436 float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel;
darth_bachious 6:ccbbf4c77d35 437 if(current_passivity==1)
darth_bachious 6:ccbbf4c77d35 438 control_torque = passivityLayer(torque_tlc,package_in);
darth_bachious 6:ccbbf4c77d35 439 else
darth_bachious 6:ccbbf4c77d35 440 control_torque = torque_tlc;
darth_bachious 6:ccbbf4c77d35 441 sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
darth_bachious 6:ccbbf4c77d35 442 }
darth_bachious 6:ccbbf4c77d35 443 else if(transparancy==2)
darth_bachious 6:ccbbf4c77d35 444 {
darth_bachious 6:ccbbf4c77d35 445 float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel;
darth_bachious 6:ccbbf4c77d35 446 if(current_passivity==1)
darth_bachious 6:ccbbf4c77d35 447 control_torque = passivityLayer(torque_tlc,package_in);
darth_bachious 6:ccbbf4c77d35 448 else
darth_bachious 6:ccbbf4c77d35 449 control_torque = torque_tlc;
darth_bachious 6:ccbbf4c77d35 450 sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,force,package_out);
darth_bachious 6:ccbbf4c77d35 451 }
darth_bachious 6:ccbbf4c77d35 452 else if(transparancy==3)
darth_bachious 6:ccbbf4c77d35 453 {
darth_bachious 6:ccbbf4c77d35 454 float torque_tlc = received_input*arm;
darth_bachious 6:ccbbf4c77d35 455 if(current_passivity==1)
darth_bachious 6:ccbbf4c77d35 456 control_torque = passivityLayer(torque_tlc,package_in);
darth_bachious 6:ccbbf4c77d35 457 else
darth_bachious 6:ccbbf4c77d35 458 control_torque = torque_tlc;
darth_bachious 6:ccbbf4c77d35 459 sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
darth_bachious 6:ccbbf4c77d35 460 }
darth_bachious 6:ccbbf4c77d35 461 }
darth_bachious 6:ccbbf4c77d35 462 else if(identity == 1)
darth_bachious 6:ccbbf4c77d35 463 {
darth_bachious 6:ccbbf4c77d35 464 if(transparancy==1)
darth_bachious 6:ccbbf4c77d35 465 {
darth_bachious 6:ccbbf4c77d35 466 float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel;
darth_bachious 6:ccbbf4c77d35 467 if(current_passivity==1)
darth_bachious 6:ccbbf4c77d35 468 control_torque = passivityLayer(torque_tlc,package_in);
darth_bachious 6:ccbbf4c77d35 469 else
darth_bachious 6:ccbbf4c77d35 470 control_torque = torque_tlc;
darth_bachious 6:ccbbf4c77d35 471 sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
darth_bachious 6:ccbbf4c77d35 472 }
darth_bachious 6:ccbbf4c77d35 473 else if(transparancy==2)
darth_bachious 6:ccbbf4c77d35 474 {
darth_bachious 6:ccbbf4c77d35 475 float torque_tlc = received_input*arm;
darth_bachious 6:ccbbf4c77d35 476 if(current_passivity==1)
darth_bachious 6:ccbbf4c77d35 477 control_torque = passivityLayer(torque_tlc,package_in);
darth_bachious 6:ccbbf4c77d35 478 else
darth_bachious 6:ccbbf4c77d35 479 control_torque = torque_tlc;
darth_bachious 6:ccbbf4c77d35 480 sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out);
darth_bachious 6:ccbbf4c77d35 481 }
darth_bachious 6:ccbbf4c77d35 482 else if(transparancy==3)
darth_bachious 6:ccbbf4c77d35 483 {
darth_bachious 6:ccbbf4c77d35 484 float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel;
darth_bachious 6:ccbbf4c77d35 485 if(current_passivity==1)
darth_bachious 6:ccbbf4c77d35 486 control_torque = passivityLayer(torque_tlc,package_in);
darth_bachious 6:ccbbf4c77d35 487 else
darth_bachious 6:ccbbf4c77d35 488 control_torque = torque_tlc;
darth_bachious 6:ccbbf4c77d35 489 sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,force,package_out);
darth_bachious 6:ccbbf4c77d35 490 }
darth_bachious 6:ccbbf4c77d35 491 }
darth_bachious 6:ccbbf4c77d35 492
darth_bachious 5:4d5b077b3fe6 493 socket.sendTo(counterpart, output, sizeof(output));
darth_bachious 5:4d5b077b3fe6 494 counter ++;
darth_bachious 6:ccbbf4c77d35 495 led=!led;
darth_bachious 6:ccbbf4c77d35 496 if(current_passivity==1)
darth_bachious 6:ccbbf4c77d35 497 led2=0;
darth_bachious 6:ccbbf4c77d35 498 else
darth_bachious 6:ccbbf4c77d35 499 led2=1;
darth_bachious 5:4d5b077b3fe6 500 controller_check = 0;
darth_bachious 6:ccbbf4c77d35 501 debug = 0;
darth_bachious 0:2f89dec3e2ab 502 }
darth_bachious 4:610b5051182a 503 osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
darth_bachious 0:2f89dec3e2ab 504 }
darth_bachious 0:2f89dec3e2ab 505 }