Bas Koster / Mbed 2 deprecated Teleoperations

Bachelor Assignment for delayed teleoperating systems

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp@10:694de5b31fd6, 2018-06-04 (annotated)

Committer:
darth_bachious
Date:
Mon Jun 04 08:31:29 2018 +0000
Revision:
10:694de5b31fd6
Parent:
9:16044ec419af
Child:
11:cb9bb3f0635d
Upgrade to controller at 100Hz;

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