Bas Koster / Mbed 2 deprecated Teleoperations

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp@15:15356e30738a, 2018-07-02 (annotated)

Committer:
darth_bachious
Date:
Mon Jul 02 08:52:05 2018 +0000
Revision:
15:15356e30738a
Parent:
14:e162b5fd0382 
Very small bugfix, now contains a energy queue. Had no impact upon till this point, but should be included for later iterations

Who changed what in which revision?

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