Bas Koster / Mbed 2 deprecated Teleoperations

Bachelor Assignment for delayed teleoperating systems

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp@5:4d5b077b3fe6, 2018-05-11 (annotated)

Committer:
darth_bachious
Date:
Fri May 11 11:15:51 2018 +0000
Revision:
5:4d5b077b3fe6
Parent:
4:610b5051182a
Child:
6:ccbbf4c77d35
Working pos-pos controller, with admittance on both sides.;

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 0:2f89dec3e2ab 6
darth_bachious 1:853939e38acd 7 //Master or Slave? 1=Master, 0=Slave
darth_bachious 2:c27b0654cffd 8 static const int identity = 0;
darth_bachious 0:2f89dec3e2ab 9
darth_bachious 0:2f89dec3e2ab 10 //network config
darth_bachious 1:853939e38acd 11 static const char* master_ip = "192.168.1.101";
darth_bachious 1:853939e38acd 12 static const char* slave_ip = "192.168.1.102";
darth_bachious 1:853939e38acd 13 static const char* MASK = "255.255.255.0";
darth_bachious 1:853939e38acd 14 static const char* GATEWAY = "192.168.1.1";
darth_bachious 0:2f89dec3e2ab 15 static const int port = 865;
darth_bachious 0:2f89dec3e2ab 16
darth_bachious 0:2f89dec3e2ab 17 //declaration of interfaces
darth_bachious 0:2f89dec3e2ab 18
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 3:376fccdc7cd6 26
darth_bachious 5:4d5b077b3fe6 27 Ticker controllerloop;
darth_bachious 0:2f89dec3e2ab 28 Ticker mainloop;
darth_bachious 0:2f89dec3e2ab 29
darth_bachious 4:610b5051182a 30 //Motor interfaces and variables
darth_bachious 3:376fccdc7cd6 31 InterruptIn EncoderA(D2);
darth_bachious 3:376fccdc7cd6 32 DigitalIn EncoderB(D3);
darth_bachious 3:376fccdc7cd6 33 DigitalOut M1_DIR(D4);
darth_bachious 5:4d5b077b3fe6 34 FastPWM M1_pwm(D5);
darth_bachious 0:2f89dec3e2ab 35
darth_bachious 4:610b5051182a 36 AnalogIn measuredForce(A5);
darth_bachious 4:610b5051182a 37
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
darth_bachious 4:610b5051182a 44 //variables related to networking
darth_bachious 1:853939e38acd 45 char data[30]= {""};
darth_bachious 0:2f89dec3e2ab 46 int size;
darth_bachious 1:853939e38acd 47 int counter = 1;
darth_bachious 1:853939e38acd 48 int counter_received = 1;
darth_bachious 0:2f89dec3e2ab 49 char * var;
darth_bachious 1:853939e38acd 50 char output[30] = {""};
darth_bachious 2:c27b0654cffd 51 float input = 0.0;
darth_bachious 4:610b5051182a 52
darth_bachious 4:610b5051182a 53 //measured variables
darth_bachious 2:c27b0654cffd 54 float angle = 0.0;
darth_bachious 5:4d5b077b3fe6 55 int encoderPos = 0;
darth_bachious 5:4d5b077b3fe6 56 int prev_encoderPos = 0;
darth_bachious 4:610b5051182a 57 float encoder_vel = 0.0;
darth_bachious 4:610b5051182a 58 float motor_vel = 0.0;
darth_bachious 4:610b5051182a 59 float force = 0.0;
darth_bachious 5:4d5b077b3fe6 60 float control_torque = 0.0;
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 5:4d5b077b3fe6 66 const float virtualMass = 0.03;
darth_bachious 5:4d5b077b3fe6 67 const float virtualDamping = 0.1;
darth_bachious 4:610b5051182a 68 const float virtualStiffness = 5;
darth_bachious 4:610b5051182a 69 const float arm = 0.05;
darth_bachious 4:610b5051182a 70
darth_bachious 2:c27b0654cffd 71
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 2:c27b0654cffd 75
darth_bachious 4:610b5051182a 76 enum States { stateCalibration, stateHoming, stateOperation};
darth_bachious 4:610b5051182a 77 int currentState = stateCalibration;
darth_bachious 4:610b5051182a 78
darth_bachious 4:610b5051182a 79 //Controller parameters
darth_bachious 5:4d5b077b3fe6 80 const float Ktl = 1; //high level controller parameters
darth_bachious 3:376fccdc7cd6 81 const float Dtl=0.1;
darth_bachious 0:2f89dec3e2ab 82
darth_bachious 4:610b5051182a 83 const float Kp = 5; //low level controller parameters
darth_bachious 4:610b5051182a 84 const float Dp = 0.05;
darth_bachious 4:610b5051182a 85
darth_bachious 4:610b5051182a 86 float u = 0.0;
darth_bachious 4:610b5051182a 87
darth_bachious 4:610b5051182a 88 //Constants
darth_bachious 0:2f89dec3e2ab 89 const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
darth_bachious 2:c27b0654cffd 90 const float RadsPerCount = (2 * PI)/(1024*10);
darth_bachious 4:610b5051182a 91 const float FORCESENSORGAIN = 15.134;
darth_bachious 4:610b5051182a 92 const int MAX_ENCODER_LEFT = 1333;
darth_bachious 4:610b5051182a 93 const int MAX_ENCODER_RIGHT = -1453;
darth_bachious 5:4d5b077b3fe6 94 const float WORKSPACEBOUND = PI/6;
darth_bachious 2:c27b0654cffd 95
darth_bachious 3:376fccdc7cd6 96 int frequency_pwm = 20000;
darth_bachious 0:2f89dec3e2ab 97
darth_bachious 4:610b5051182a 98 //Time delay related variables
darth_bachious 4:610b5051182a 99 float timedelay = 0.04; //SECONDS
darth_bachious 2:c27b0654cffd 100 std::vector<float> delayArrayINPUT(ceil(timedelay/looptime),0.0);
darth_bachious 2:c27b0654cffd 101
darth_bachious 4:610b5051182a 102 Timer t;
darth_bachious 4:610b5051182a 103
darth_bachious 3:376fccdc7cd6 104 //FUNCTIONS START HERE
darth_bachious 3:376fccdc7cd6 105
darth_bachious 3:376fccdc7cd6 106 void encoderFunctionA()
darth_bachious 3:376fccdc7cd6 107 {
darth_bachious 3:376fccdc7cd6 108 if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
darth_bachious 3:376fccdc7cd6 109 encoderPos++;
darth_bachious 3:376fccdc7cd6 110 } else {
darth_bachious 3:376fccdc7cd6 111 encoderPos--;
darth_bachious 3:376fccdc7cd6 112 }
darth_bachious 3:376fccdc7cd6 113 }
darth_bachious 4:610b5051182a 114 double velocityFilter(float x)
darth_bachious 4:610b5051182a 115 {
darth_bachious 4:610b5051182a 116 double y = 0.0; //Output
darth_bachious 4:610b5051182a 117 static double y_1 = 0.0; //Output last loop
darth_bachious 4:610b5051182a 118 static double y_2 = 0.0; //Output 2 loops ago
darth_bachious 4:610b5051182a 119 static double x_1 = 0.0; //Input last loop
darth_bachious 4:610b5051182a 120 static double x_2 = 0.0; //Input two loops ago
darth_bachious 3:376fccdc7cd6 121
darth_bachious 4:610b5051182a 122 //Finite difference equation for 2nd order Butterworth low-pass
darth_bachious 4:610b5051182a 123 y = -a[1]*y_1 - a[2]*y_2 + x*b[0] + x_1*b[1] + x_2*b[2];
darth_bachious 4:610b5051182a 124 y_2 = y_1;
darth_bachious 4:610b5051182a 125 y_1 = y;
darth_bachious 4:610b5051182a 126 x_2 = x_1;
darth_bachious 4:610b5051182a 127 x_1 = x;
darth_bachious 4:610b5051182a 128 return (float)y;
darth_bachious 4:610b5051182a 129 }
darth_bachious 0:2f89dec3e2ab 130
darth_bachious 0:2f89dec3e2ab 131 void inet_eth(){
darth_bachious 1:853939e38acd 132 if(identity==1)
darth_bachious 1:853939e38acd 133 {
darth_bachious 1:853939e38acd 134 eth.init(master_ip, MASK,GATEWAY);
darth_bachious 1:853939e38acd 135 eth.connect();
darth_bachious 0:2f89dec3e2ab 136
darth_bachious 1:853939e38acd 137 socket.bind(port);
darth_bachious 1:853939e38acd 138 counterpart.set_address(slave_ip,port);
darth_bachious 1:853939e38acd 139 }
darth_bachious 1:853939e38acd 140 else if(identity==0)
darth_bachious 1:853939e38acd 141 {
darth_bachious 1:853939e38acd 142 eth.init(slave_ip, MASK,GATEWAY);
darth_bachious 1:853939e38acd 143 eth.connect();
darth_bachious 1:853939e38acd 144
darth_bachious 1:853939e38acd 145 socket.bind(port);
darth_bachious 1:853939e38acd 146 counterpart.set_address(master_ip,port);
darth_bachious 1:853939e38acd 147 }
darth_bachious 1:853939e38acd 148
darth_bachious 0:2f89dec3e2ab 149 }
darth_bachious 0:2f89dec3e2ab 150
darth_bachious 0:2f89dec3e2ab 151 void inet_USB(){
darth_bachious 2:c27b0654cffd 152 pc.baud(115200);
darth_bachious 0:2f89dec3e2ab 153 }
darth_bachious 0:2f89dec3e2ab 154
darth_bachious 0:2f89dec3e2ab 155
darth_bachious 0:2f89dec3e2ab 156 void end_eth(){
darth_bachious 0:2f89dec3e2ab 157 socket.close();
darth_bachious 0:2f89dec3e2ab 158 eth.disconnect();
darth_bachious 0:2f89dec3e2ab 159 }
darth_bachious 0:2f89dec3e2ab 160
darth_bachious 5:4d5b077b3fe6 161 void controllertrigger()
darth_bachious 0:2f89dec3e2ab 162 {
darth_bachious 5:4d5b077b3fe6 163 controller_check = 1;
darth_bachious 0:2f89dec3e2ab 164 }
darth_bachious 2:c27b0654cffd 165
darth_bachious 4:610b5051182a 166
darth_bachious 2:c27b0654cffd 167 float update_delay(std::vector<float>&array, float new_value)
darth_bachious 2:c27b0654cffd 168 {
darth_bachious 3:376fccdc7cd6 169 float return_value = array[1];
darth_bachious 3:376fccdc7cd6 170 for (int i=0; i<array.size()-1; ++i)
darth_bachious 3:376fccdc7cd6 171 {
darth_bachious 3:376fccdc7cd6 172 array[i]=array[i+1];
darth_bachious 3:376fccdc7cd6 173 }
darth_bachious 3:376fccdc7cd6 174 array.back() = new_value;
darth_bachious 3:376fccdc7cd6 175 return return_value;
darth_bachious 3:376fccdc7cd6 176 }
darth_bachious 3:376fccdc7cd6 177
darth_bachious 3:376fccdc7cd6 178 void limit(float &x, float lower, float upper)
darth_bachious 3:376fccdc7cd6 179 {
darth_bachious 3:376fccdc7cd6 180 if (x > upper)
darth_bachious 3:376fccdc7cd6 181 x = upper;
darth_bachious 3:376fccdc7cd6 182 if (x < lower)
darth_bachious 3:376fccdc7cd6 183 x = lower;
darth_bachious 3:376fccdc7cd6 184 }
darth_bachious 3:376fccdc7cd6 185
darth_bachious 4:610b5051182a 186 void motor_update(float PWM) //angle required to safeguard it from crashing into its stops
darth_bachious 4:610b5051182a 187 {
darth_bachious 4:610b5051182a 188 limit(PWM,-1.0f,1.0f);
darth_bachious 4:610b5051182a 189 if(PWM >= 0.0f)
darth_bachious 4:610b5051182a 190 {
darth_bachious 4:610b5051182a 191 M1_DIR = false;
darth_bachious 4:610b5051182a 192 M1_pwm = PWM;
darth_bachious 4:610b5051182a 193 } else {
darth_bachious 4:610b5051182a 194 M1_DIR = true;
darth_bachious 4:610b5051182a 195 M1_pwm = -PWM;
darth_bachious 4:610b5051182a 196 }
darth_bachious 4:610b5051182a 197 }
darth_bachious 4:610b5051182a 198
darth_bachious 4:610b5051182a 199 void sensorUpdate()
darth_bachious 4:610b5051182a 200 {
darth_bachious 4:610b5051182a 201 angle = encoderPos * RadsPerCount;
darth_bachious 5:4d5b077b3fe6 202 encoder_vel = (encoderPos - prev_encoderPos)/ADDMlooptime; //careful, this function should be called every 1/5000 seconds
darth_bachious 4:610b5051182a 203 motor_vel = velocityFilter(encoder_vel * RadsPerCount);
darth_bachious 4:610b5051182a 204 prev_encoderPos = encoderPos;
darth_bachious 4:610b5051182a 205 force = -FORCESENSORGAIN*2.0f*(measuredForce - 0.5f); //Measured force
darth_bachious 4:610b5051182a 206 }
darth_bachious 4:610b5051182a 207
darth_bachious 4:610b5051182a 208 void doCalibration()
darth_bachious 4:610b5051182a 209 {
darth_bachious 4:610b5051182a 210 u = 0.09;
darth_bachious 4:610b5051182a 211 motor_update(u);
darth_bachious 4:610b5051182a 212 led2=1;
darth_bachious 4:610b5051182a 213 led=0;
darth_bachious 4:610b5051182a 214 //switching states
darth_bachious 4:610b5051182a 215 if((abs(motor_vel)<0.001f)&& t.read()>1.0)
darth_bachious 3:376fccdc7cd6 216 {
darth_bachious 4:610b5051182a 217 encoderPos = MAX_ENCODER_LEFT;
darth_bachious 4:610b5051182a 218 ref_angle = encoderPos * RadsPerCount;
darth_bachious 4:610b5051182a 219 currentState = stateHoming;
darth_bachious 4:610b5051182a 220 t.stop();
darth_bachious 4:610b5051182a 221 }
darth_bachious 4:610b5051182a 222 }
darth_bachious 4:610b5051182a 223
darth_bachious 4:610b5051182a 224 void doHoming()
darth_bachious 4:610b5051182a 225 {
darth_bachious 4:610b5051182a 226 led2=0;
darth_bachious 4:610b5051182a 227 led=0;
darth_bachious 4:610b5051182a 228 ref_vel = -0.2;
darth_bachious 4:610b5051182a 229 if(ref_angle > 0.0)
darth_bachious 4:610b5051182a 230 {
darth_bachious 5:4d5b077b3fe6 231 ref_angle += ref_vel*ADDMlooptime; //careful, this function should be called every 1/50 seconds
darth_bachious 4:610b5051182a 232 }
darth_bachious 4:610b5051182a 233 else
darth_bachious 4:610b5051182a 234 {
darth_bachious 4:610b5051182a 235 ref_angle = 0.0;
darth_bachious 4:610b5051182a 236 ref_vel = 0.0;
darth_bachious 4:610b5051182a 237 }
darth_bachious 4:610b5051182a 238 u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);
darth_bachious 4:610b5051182a 239 motor_update(u);
darth_bachious 4:610b5051182a 240
darth_bachious 4:610b5051182a 241
darth_bachious 4:610b5051182a 242
darth_bachious 4:610b5051182a 243 //switching states
darth_bachious 4:610b5051182a 244 if ((abs(encoderPos)<20)&&abs((ref_vel - motor_vel)<0.02))
darth_bachious 4:610b5051182a 245 {
darth_bachious 4:610b5051182a 246 currentState = stateOperation;
darth_bachious 4:610b5051182a 247 motor_update(0.0);
darth_bachious 5:4d5b077b3fe6 248 led2=1;
darth_bachious 4:610b5051182a 249 }
darth_bachious 3:376fccdc7cd6 250
darth_bachious 4:610b5051182a 251 }
darth_bachious 4:610b5051182a 252
darth_bachious 4:610b5051182a 253 void doOperation()
darth_bachious 4:610b5051182a 254 {
darth_bachious 5:4d5b077b3fe6 255 ref_acc = (force*arm + control_torque- virtualDamping*ref_vel)/virtualMass;
darth_bachious 5:4d5b077b3fe6 256 ref_vel += ref_acc*ADDMlooptime;
darth_bachious 5:4d5b077b3fe6 257 ref_angle += ref_vel*ADDMlooptime;
darth_bachious 5:4d5b077b3fe6 258 if(ref_angle > WORKSPACEBOUND)
darth_bachious 5:4d5b077b3fe6 259 {
darth_bachious 5:4d5b077b3fe6 260 ref_vel = 0.0;
darth_bachious 5:4d5b077b3fe6 261 ref_acc = 0.0;
darth_bachious 5:4d5b077b3fe6 262 ref_angle = WORKSPACEBOUND;
darth_bachious 5:4d5b077b3fe6 263 } else if(ref_angle < -WORKSPACEBOUND)
darth_bachious 5:4d5b077b3fe6 264 {
darth_bachious 5:4d5b077b3fe6 265 ref_vel = 0.0;
darth_bachious 5:4d5b077b3fe6 266 ref_acc = 0.0;
darth_bachious 5:4d5b077b3fe6 267 ref_angle = -WORKSPACEBOUND;
darth_bachious 5:4d5b077b3fe6 268 }
darth_bachious 5:4d5b077b3fe6 269 u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);
darth_bachious 4:610b5051182a 270 motor_update(u);
darth_bachious 5:4d5b077b3fe6 271 }
darth_bachious 5:4d5b077b3fe6 272
darth_bachious 5:4d5b077b3fe6 273 void loopfunction()
darth_bachious 5:4d5b077b3fe6 274 {
darth_bachious 5:4d5b077b3fe6 275 sensorUpdate();
darth_bachious 5:4d5b077b3fe6 276 switch(currentState)
darth_bachious 5:4d5b077b3fe6 277 {
darth_bachious 5:4d5b077b3fe6 278 case stateCalibration:
darth_bachious 5:4d5b077b3fe6 279 doCalibration();
darth_bachious 5:4d5b077b3fe6 280 break;
darth_bachious 5:4d5b077b3fe6 281 case stateHoming:
darth_bachious 5:4d5b077b3fe6 282 doHoming();
darth_bachious 5:4d5b077b3fe6 283 break;
darth_bachious 5:4d5b077b3fe6 284 case stateOperation:
darth_bachious 5:4d5b077b3fe6 285 doOperation();
darth_bachious 5:4d5b077b3fe6 286 break;
darth_bachious 5:4d5b077b3fe6 287 }
darth_bachious 4:610b5051182a 288 }
darth_bachious 0:2f89dec3e2ab 289
darth_bachious 0:2f89dec3e2ab 290 void receiveUDP(void const *argument){
darth_bachious 4:610b5051182a 291 while(true)
darth_bachious 4:610b5051182a 292 {
darth_bachious 0:2f89dec3e2ab 293 size = socket.receiveFrom(client, data, sizeof(data));
darth_bachious 4:610b5051182a 294 if(size > 0)
darth_bachious 4:610b5051182a 295 {
darth_bachious 0:2f89dec3e2ab 296 data[size] = '\0';
darth_bachious 2:c27b0654cffd 297 if(size>5) //first check, an minimum amount of data must have arrived
darth_bachious 4:610b5051182a 298 {
darth_bachious 2:c27b0654cffd 299 var = strtok(data,"; ");
darth_bachious 2:c27b0654cffd 300 if(counter_received < atof(var)) //second check, data must be newer
darth_bachious 4:610b5051182a 301 {
darth_bachious 2:c27b0654cffd 302 counter_received = atof(var);
darth_bachious 2:c27b0654cffd 303 var = strtok(NULL,"; ");
darth_bachious 2:c27b0654cffd 304 input = atof(var);
darth_bachious 4:610b5051182a 305 pc.printf("input: %f \n\r",input);
darth_bachious 4:610b5051182a 306 }
darth_bachious 0:2f89dec3e2ab 307 }
darth_bachious 0:2f89dec3e2ab 308 }
darth_bachious 0:2f89dec3e2ab 309 }
darth_bachious 4:610b5051182a 310 }
darth_bachious 0:2f89dec3e2ab 311
darth_bachious 0:2f89dec3e2ab 312 osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);
darth_bachious 0:2f89dec3e2ab 313
darth_bachious 0:2f89dec3e2ab 314 int main(){
darth_bachious 1:853939e38acd 315 inet_eth();
darth_bachious 0:2f89dec3e2ab 316 inet_USB();
darth_bachious 0:2f89dec3e2ab 317
darth_bachious 0:2f89dec3e2ab 318 osThreadCreate(osThread(receiveUDP), NULL);
darth_bachious 0:2f89dec3e2ab 319 led2=1;
darth_bachious 0:2f89dec3e2ab 320 led=1;
darth_bachious 5:4d5b077b3fe6 321
darth_bachious 5:4d5b077b3fe6 322 //Set all interrupt requests to 2nd place priority
darth_bachious 5:4d5b077b3fe6 323 for(int n = 0; n < 86; n++) {
darth_bachious 5:4d5b077b3fe6 324 NVIC_SetPriority((IRQn)n,1);
darth_bachious 5:4d5b077b3fe6 325 }
darth_bachious 5:4d5b077b3fe6 326 //Set out motor encoder interrupt to 1st place priority
darth_bachious 5:4d5b077b3fe6 327 NVIC_SetPriority(PORTB_IRQn,0);
darth_bachious 5:4d5b077b3fe6 328
darth_bachious 5:4d5b077b3fe6 329 controllerloop.attach(&controllertrigger,looptime);
darth_bachious 5:4d5b077b3fe6 330 mainloop.attach(&loopfunction,ADDMlooptime);
darth_bachious 5:4d5b077b3fe6 331
darth_bachious 4:610b5051182a 332 M1_pwm.period(1.0/frequency_pwm);
darth_bachious 4:610b5051182a 333 EncoderA.rise(&encoderFunctionA);
darth_bachious 0:2f89dec3e2ab 334
darth_bachious 4:610b5051182a 335 t.start();
darth_bachious 3:376fccdc7cd6 336
darth_bachious 0:2f89dec3e2ab 337 while(true){
darth_bachious 5:4d5b077b3fe6 338 if(controller_check==1){
darth_bachious 5:4d5b077b3fe6 339 float reference = update_delay(delayArrayINPUT,input);
darth_bachious 5:4d5b077b3fe6 340 control_torque = Ktl*(reference-angle) - Dtl*motor_vel;
darth_bachious 3:376fccdc7cd6 341
darth_bachious 5:4d5b077b3fe6 342 sprintf(output, "%i;%f",counter,angle);
darth_bachious 5:4d5b077b3fe6 343 socket.sendTo(counterpart, output, sizeof(output));
darth_bachious 5:4d5b077b3fe6 344 counter ++;
darth_bachious 5:4d5b077b3fe6 345 led=!led;
darth_bachious 5:4d5b077b3fe6 346 controller_check = 0;
darth_bachious 5:4d5b077b3fe6 347 //pc.printf("force: %f \n\r",force);
darth_bachious 0:2f89dec3e2ab 348 }
darth_bachious 4:610b5051182a 349 osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
darth_bachious 0:2f89dec3e2ab 350 }
darth_bachious 0:2f89dec3e2ab 351 }