Bachelor Assignment for delayed teleoperating systems
Dependencies: EthernetInterface FastPWM mbed-rtos mbed MODSERIAL
Diff: main.cpp
- Revision:
- 6:ccbbf4c77d35
- Parent:
- 5:4d5b077b3fe6
- Child:
- 7:984f363f5e87
--- a/main.cpp Fri May 11 11:15:51 2018 +0000 +++ b/main.cpp Tue May 15 11:28:18 2018 +0000 @@ -3,9 +3,10 @@ #include "rtos.h" #include <vector> #include "FastPWM.h" +#include <cmath> //Master or Slave? 1=Master, 0=Slave -static const int identity = 0; +static const int identity = 1; //network config static const char* master_ip = "192.168.1.101"; @@ -15,7 +16,6 @@ static const int port = 865; //declaration of interfaces - DigitalOut led(LED_GREEN); DigitalOut led2(LED_RED); EthernetInterface eth; //network @@ -23,24 +23,23 @@ UDPSocket socket; //socket to receive data on Endpoint client; //The virtual other side, not to send actual information to Endpoint counterpart; //The actual other side, this is where the information should go to - +InterruptIn Button1(SW2); +InterruptIn Button2(SW3); Ticker controllerloop; Ticker mainloop; +DigitalOut debug(D11); //Motor interfaces and variables InterruptIn EncoderA(D2); -DigitalIn EncoderB(D3); +DigitalIn EncoderB(D3); DigitalOut M1_DIR(D4); FastPWM M1_pwm(D5); - AnalogIn measuredForce(A5); - //Low pass filter filter coeffs, 2nd order, 50 Hz double b[3] = {0.020083365564211, 0.020083365564211*2, 0.020083365564211}; double a[3] = {1.00000000000000, -1.561018075, 0.64135153805}; - //variables related to networking char data[30]= {""}; int size; @@ -58,32 +57,47 @@ float motor_vel = 0.0; float force = 0.0; float control_torque = 0.0; +float force_offset = 0.5; //Reference, used in admittance float ref_angle = 0.0; float ref_vel = 0.0; float ref_acc = 0.0; -const float virtualMass = 0.03; -const float virtualDamping = 0.1; +const float virtualMass = 0.005; +const float virtualDamping = 0.05; const float virtualStiffness = 5; -const float arm = 0.05; +const float arm = 0.07; - +//Controller required variables bool controller_check = 0; const float looptime = 1.0/50; //50Hz, for the controller const float ADDMlooptime = 1.0/5000; //5KHz, for the admittance controller - -enum States { stateCalibration, stateHoming, stateOperation}; +enum States {stateCalibration, stateHoming, stateOperation}; int currentState = stateCalibration; +int transparancy = 1; // 1=Pos-Pos, 2=Pos-Force, 3=Force-Pos +int passivity =0; // 0=off, 1=on +int received_transparancy = 1; +int received_passivity = 0; //Controller parameters const float Ktl = 1; //high level controller parameters const float Dtl=0.1; - const float Kp = 5; //low level controller parameters const float Dp = 0.05; +float u = 0.0; //serves as input variable for the motor; -float u = 0.0; +//passivity layer constant +const float beta = 0.15; +const float Hd = 0.5; +const float alpha = 10; + +//passivity layer variables +float tank = 0.0; +float prev_ref_angle = 0.0; +float package_out = 0.0; +float received_package = 0.0; +float prev_torque = 0.0; + //Constants const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679; @@ -93,12 +107,15 @@ const int MAX_ENCODER_RIGHT = -1453; const float WORKSPACEBOUND = PI/6; -int frequency_pwm = 20000; +const int frequency_pwm = 20000; //Time delay related variables float timedelay = 0.04; //SECONDS -std::vector<float> delayArrayINPUT(ceil(timedelay/looptime),0.0); - +int delaysteps = timedelay/looptime; +std::vector<float> delayArrayINPUT(max(delaysteps,1),0.0); +std::vector<float> delayArrayMODE(max(delaysteps,1),0.0); +std::vector<float> delayArrayPASS(max(delaysteps,1),0.0); +std::vector<float> delayArrayENERGY(max(delaysteps,1),0.0); Timer t; //FUNCTIONS START HERE @@ -202,17 +219,17 @@ encoder_vel = (encoderPos - prev_encoderPos)/ADDMlooptime; //careful, this function should be called every 1/5000 seconds motor_vel = velocityFilter(encoder_vel * RadsPerCount); prev_encoderPos = encoderPos; - force = -FORCESENSORGAIN*2.0f*(measuredForce - 0.5f); //Measured force + force = -FORCESENSORGAIN*2.0f*(measuredForce - force_offset); //Measured force } void doCalibration() { u = 0.09; motor_update(u); - led2=1; - led=0; + led2=0; + led=1; //switching states - if((abs(motor_vel)<0.001f)&& t.read()>1.0) + if((abs(motor_vel)<0.001f)&& t.read()>3.0f) { encoderPos = MAX_ENCODER_LEFT; ref_angle = encoderPos * RadsPerCount; @@ -226,7 +243,7 @@ led2=0; led=0; ref_vel = -0.2; - if(ref_angle > 0.0) + if(ref_angle > 0.0f) { ref_angle += ref_vel*ADDMlooptime; //careful, this function should be called every 1/50 seconds } @@ -238,14 +255,14 @@ u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel); motor_update(u); - - //switching states - if ((abs(encoderPos)<20)&&abs((ref_vel - motor_vel)<0.02)) + if ((abs(encoderPos)<20)&&abs((ref_vel - motor_vel)<0.02f)) { currentState = stateOperation; + force_offset = measuredForce; motor_update(0.0); led2=1; + led=1; } } @@ -268,6 +285,8 @@ } u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel); motor_update(u); + + //no switching states for now } void loopfunction() @@ -287,6 +306,62 @@ } } +void updateTransparency() +{ + transparancy++; + if(transparancy>3) + transparancy = 1; +} + +void updatePassivity() +{ + passivity++; + if(passivity>1) + passivity = 0; +} + +float passivityLayer(float Ftl, float E_in) +{ + tank = tank + E_in - prev_torque*(ref_angle-prev_ref_angle); + if(tank>0.0f) + { + package_out = tank*beta; + tank = tank - package_out; + } else + package_out = 0.0; + + float FMAX1 = 0.0; + if(tank>0.0f) + { + float FMAX1 = abs(Ftl); + } + float FMAX2 = abs(tank/(ref_vel*looptime)); + + float FMAX3 = 0.15; + + prev_ref_angle = ref_angle; + + float Ftlc = 0.0; + + if((tank<Hd)&&(identity==1)) + { + Ftlc = - alpha*(Hd-tank)*ref_vel; + } + //pc.printf("Ftlc: %f\r\n",Ftlc); + //pc.printf("tank: %f\r\n",tank); + if(Ftl>=0.0f) + { + prev_torque = min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc; + return min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc; //min() only takes to arguments, so nested min()'s + } + else + { + prev_torque = -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))-Ftlc; + return -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))-Ftlc; + } + +} + void receiveUDP(void const *argument){ while(true) { @@ -294,6 +369,7 @@ if(size > 0) { data[size] = '\0'; + pc.printf("data:%s\r\n",data); if(size>5) //first check, an minimum amount of data must have arrived { var = strtok(data,"; "); @@ -301,8 +377,13 @@ { counter_received = atof(var); var = strtok(NULL,"; "); + received_transparancy = atof(var); + var = strtok(NULL,"; "); + received_passivity = atof(var); + var = strtok(NULL,"; "); input = atof(var); - pc.printf("input: %f \n\r",input); + var = strtok(NULL,"; "); + received_package += atof(var); } } } @@ -319,32 +400,105 @@ led2=1; led=1; - //Set all interrupt requests to 2nd place priority + //Set all interrupt requests to 2nd place priority for(int n = 0; n < 86; n++) { NVIC_SetPriority((IRQn)n,1); } //Set out motor encoder interrupt to 1st place priority NVIC_SetPriority(PORTB_IRQn,0); - + controllerloop.attach(&controllertrigger,looptime); mainloop.attach(&loopfunction,ADDMlooptime); M1_pwm.period(1.0/frequency_pwm); EncoderA.rise(&encoderFunctionA); + Button1.fall(&updateTransparency); + Button2.fall(&updatePassivity); + t.start(); while(true){ if(controller_check==1){ - float reference = update_delay(delayArrayINPUT,input); - control_torque = Ktl*(reference-angle) - Dtl*motor_vel; + debug = 1; + float received_input = update_delay(delayArrayINPUT,input); + float current_transparancy = update_delay(delayArrayMODE,received_transparancy); + float current_passivity = update_delay(delayArrayPASS,received_passivity); + float package_in = update_delay(delayArrayENERGY,received_package); + received_package = 0; //IMPORTANT, WILL EXPLODE OTHERWISE - sprintf(output, "%i;%f",counter,angle); + if(identity==0) + { + transparancy = current_transparancy; + passivity = current_passivity; + if(transparancy==1) + { + float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel; + if(current_passivity==1) + control_torque = passivityLayer(torque_tlc,package_in); + else + control_torque = torque_tlc; + sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out); + } + else if(transparancy==2) + { + float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel; + if(current_passivity==1) + control_torque = passivityLayer(torque_tlc,package_in); + else + control_torque = torque_tlc; + sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,force,package_out); + } + else if(transparancy==3) + { + float torque_tlc = received_input*arm; + if(current_passivity==1) + control_torque = passivityLayer(torque_tlc,package_in); + else + control_torque = torque_tlc; + sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out); + } + } + else if(identity == 1) + { + if(transparancy==1) + { + float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel; + if(current_passivity==1) + control_torque = passivityLayer(torque_tlc,package_in); + else + control_torque = torque_tlc; + sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out); + } + else if(transparancy==2) + { + float torque_tlc = received_input*arm; + if(current_passivity==1) + control_torque = passivityLayer(torque_tlc,package_in); + else + control_torque = torque_tlc; + sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,ref_angle,package_out); + } + else if(transparancy==3) + { + float torque_tlc = Ktl*(received_input-ref_angle) - Dtl*ref_vel; + if(current_passivity==1) + control_torque = passivityLayer(torque_tlc,package_in); + else + control_torque = torque_tlc; + sprintf(output,"%i;%i;%i;%f;%f",counter,transparancy,passivity,force,package_out); + } + } + socket.sendTo(counterpart, output, sizeof(output)); counter ++; - led=!led; + led=!led; + if(current_passivity==1) + led2=0; + else + led2=1; controller_check = 0; - //pc.printf("force: %f \n\r",force); + debug = 0; } osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK }