Bas Koster / Mbed 2 deprecated Teleoperations

Bachelor Assignment for delayed teleoperating systems

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp@4:610b5051182a, 2018-05-11 (annotated)

Committer:
darth_bachious
Date:
Fri May 11 08:25:05 2018 +0000
Revision:
4:610b5051182a
Parent:
3:376fccdc7cd6
Child:
5:4d5b077b3fe6
State machine, pos-pos impedance control;

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