Dependencies: EthernetInterface FastPWM mbed-rtos mbed MODSERIAL
Diff: main.cpp
- Revision:
- 12:5c08ffe8ad1d
- Parent:
- 11:cb9bb3f0635d
- Child:
- 13:787cabccb2be
--- a/main.cpp Wed Jun 06 07:06:14 2018 +0000
+++ b/main.cpp Tue Jun 12 08:08:38 2018 +0000
@@ -7,7 +7,7 @@
#include "MODSERIAL.h"
//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";
@@ -19,10 +19,10 @@
//declaration of interfaces
-DigitalOut led(LED_GREEN);
+DigitalOut led(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);
-EthernetInterface eth; //network
+EthernetInterface eth; //network
Serial pc(USBTX, USBRX);//create PC interface
UDPSocket socket; //socket to receive data on
Endpoint client; //The virtual other side, not to send actual information to
@@ -77,11 +77,11 @@
const float arm = 0.085;
const float max_velocity = 0.8;
-//Controller required variables
+//Controller required variables
bool controller_check = 0;
const float sample_frequency = 200;
-float looptime = 1.0/(sample_frequency); //50Hz, for the controller
-const float ADDMlooptime = 1.0/2500; //2.5KHz, for the admittance controller
+float looptime = 1.0/(sample_frequency); //200Hz, for the controller
+const float ADDMlooptime = 1.0/2500; //2.5KHz, for the admittance controller
enum States {stateCalibration, stateHoming, stateOperation};
int currentState = stateCalibration;
int transparancy = 1; // 1=Pos-Pos, 2=Pos-Force, 3=Force-Pos
@@ -90,11 +90,11 @@
int received_passivity = 0;
//Controller parameters
-const float Ktl = 4.5; //high level controller parameters
+const float Ktl = 4.5; //high level controller parameters
const float Dtl=0.1;
-const float Kp = 100; //low level controller parameters
+const float Kp = 100; //low level controller parameters
const float Dp = 0.5;
-float u = 0.0; //serves as input variable for the motor;
+float u = 0.0; //serves as input variable for the motor;
//passivity layer constant
const float beta = 0.2041;
@@ -120,7 +120,7 @@
const int frequency_pwm = 20000;
//Time delay related variables
-float timedelay = 0.2; //SECONDS
+float timedelay = 0.1;//SECONDS
int delaysteps = timedelay/looptime;
std::vector<float> delayArrayINPUT(max(delaysteps,1),0.0);
std::vector<float> delayArrayMODE(max(delaysteps,1),0.0);
@@ -131,13 +131,13 @@
//FUNCTIONS START HERE
void encoderFunctionA()
- {
- if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
- encoderPos++;
- } else {
- encoderPos--;
- }
+{
+ if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
+ encoderPos++;
+ } else {
+ encoderPos--;
}
+}
double velocityFilter(float x)
{
double y = 0.0; //Output
@@ -155,73 +155,71 @@
return (float)y;
}
-void inet_eth(){
- if(identity==1)
- {
+void inet_eth()
+{
+ if(identity==1) {
eth.init(master_ip, MASK,GATEWAY);
eth.connect();
-
+
socket.bind(port);
counterpart.set_address(slave_ip,port);
laptop.set_address(laptop_IP,port);
- }
- else if(identity==0)
- {
+ } else if(identity==0) {
eth.init(slave_ip, MASK,GATEWAY);
eth.connect();
-
+
socket.bind(port);
counterpart.set_address(master_ip,port);
laptop.set_address(laptop_IP,port+1);
- }
-
}
-void inet_USB(){
+}
+
+void inet_USB()
+{
pc.baud(115200);
- }
-
+}
+
-void end_eth(){
+void end_eth()
+{
socket.close();
eth.disconnect();
- }
+}
void controllertrigger()
- {
- controller_check = 1;
- }
-
-
+{
+ controller_check = 1;
+}
+
+
float update_delay(std::vector<float>&array, float new_value)
- {
+{
float return_value = array[0];
- for (int i=0; i<array.size()-1; ++i)
- {
+ for (int i=0; i<array.size()-1; ++i) {
array[i]=array[i+1];
}
array.back() = new_value;
return return_value;
- }
-
+}
+
void limit(float &x, float lower, float upper)
{
if (x > upper)
x = upper;
if (x < lower)
- x = lower;
+ x = lower;
}
void motor_update(float PWM) //angle required to safeguard it from crashing into its stops
{
limit(PWM,-1.0f,1.0f);
- if(PWM >= 0.0f)
- {
+ if(PWM >= 0.0f) {
M1_DIR = false;
M1_pwm = PWM;
} else {
M1_DIR = true;
- M1_pwm = -PWM;
+ M1_pwm = -PWM;
}
}
@@ -232,8 +230,8 @@
motor_vel = velocityFilter(encoder_vel * RadsPerCount);
prev_encoderPos = encoderPos;
force = -FORCESENSORGAIN*2.0f*(measuredForce - force_offset); //Measured force
-}
-
+}
+
void doCalibration()
{
u = -0.15;
@@ -241,13 +239,12 @@
led2=0;
led=1;
//switching states
- if((abs(motor_vel)<0.001f)&& t.read()>3.0f)
- {
- encoderPos = MAX_ENCODER_RIGHT;
+ if((abs(motor_vel)<0.001f)&& t.read()>3.0f) {
+ encoderPos = MAX_ENCODER_RIGHT - (1-identity)*180; //to make up for the difference in setups
ref_angle = encoderPos * RadsPerCount;
currentState = stateHoming;
t.stop();
- }
+ }
}
void doHoming()
@@ -255,28 +252,24 @@
led2=0;
led=0;
ref_vel = 0.2;
- if(ref_angle < 0.0f)
- {
+ if(ref_angle < 0.0f) {
ref_angle += ref_vel*ADDMlooptime; //careful, this function should be called every 1/50 seconds
- }
- else
- {
+ } else {
ref_angle = 0.0;
ref_vel = 0.0;
}
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.02f))
- {
+ if ((abs(encoderPos)<20)&&abs((ref_vel - motor_vel)<0.02f)) {
currentState = stateOperation;
force_offset = measuredForce;
motor_update(0.0);
led2=1;
led=1;
}
-
+
}
void doOperation()
@@ -288,39 +281,36 @@
else if(ref_vel<-max_velocity)
ref_vel=-max_velocity;
-
+
ref_angle += ref_vel*ADDMlooptime;
- if(ref_angle > WORKSPACEBOUND)
- {
+ if(ref_angle > WORKSPACEBOUND) {
ref_vel = 0.0;
ref_acc = 0.0;
ref_angle = WORKSPACEBOUND;
- } else if(ref_angle < -WORKSPACEBOUND)
- {
+ } else if(ref_angle < -WORKSPACEBOUND) {
ref_vel = 0.0;
ref_acc = 0.0;
ref_angle = -WORKSPACEBOUND;
}
- u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);
+ u = Kp*(ref_angle - angle) + Dp*(ref_vel - motor_vel);
motor_update(u);
-
+
//no switching states for now
}
void loopfunction()
{
sensorUpdate();
- switch(currentState)
- {
+ switch(currentState) {
case stateCalibration:
- doCalibration();
- break;
+ doCalibration();
+ break;
case stateHoming:
- doHoming();
- break;
+ doHoming();
+ break;
case stateOperation:
- doOperation();
- break;
+ doOperation();
+ break;
}
}
@@ -335,54 +325,42 @@
{
passivity++;
if(passivity>1)
- passivity = 0;
+ passivity = 0;
}
float passivityLayer(float Ftl, float E_in)
{
tank = tank + E_in - prev_torque*(ref_angle-prev_ref_angle);
- if(tank>0.0f)
- {
+ 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)
- {
+ if(tank>0.0f) {
FMAX1 = abs(Ftl);
}
float FMAX2 = abs(tank/(ref_vel*looptime));
float FMAX3 = 20.0;
- if(identity==0)
- {
+ if(identity==0) {
FMAX3 = 0.7;
}
- prev_ref_angle = ref_angle;
-
+ prev_ref_angle = ref_angle;
+
float Ftlc = 0.0;
-
- if((tank<Hd)&&(identity==1))
- {
+
+ if((tank<Hd)&&(identity==1)) {
Ftlc = - alpha*(Hd-tank)*ref_vel;
- }
- //pc.printf("%f,%f,%f,%f\r\n",tank,Ftl,min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3)),Ftlc);
- //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;
+ }
+ 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
- //return 0.0;
- }
- else
- {
+ } else {
prev_torque = -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
- //return 0.0;
- return -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
+ return -min(min(abs(Ftl),FMAX1),min(FMAX2,FMAX3))+Ftlc;
}
-
+
}
void generateOutput(float variable)
@@ -392,8 +370,6 @@
memcpy(&output[8],&passivity,4);
memcpy(&output[12],&variable,4);
memcpy(&output[16],&package_out,4);
- //pc.printf("output:%i,%i,%i,%f,%f\r\n",counter,transparancy,passivity,variable,package_out);
-
}
void generateStatus()
{
@@ -404,29 +380,32 @@
memcpy(&status[16],&percentage_received,4);
}
-void receiveUDP(void const *argument){
- while(true)
- {
+void receiveUDP(void const *argument)
+{
+ while(true) {
size = socket.receiveFrom(client, data, sizeof(data));
- if(size > 0)
- {
+ if(size > 0) {
data[size] = '\0';
- if(size>18) //first check, an minimum amount of data must have arrived
- {
+ if(size>18) { //first check, an minimum amount of data must have arrived
memcpy(&check,&data[0],4);
- if(counter_received < check) //second check, data must be newer
- {
+ if(counter_received < check) { //second check, data must be newer
+ if((counter_received == check-1)&&(counter_not_missed<100)) {
+ counter_not_missed++;
+ } else if((counter_not_missed)>10){
+ counter_not_missed = counter_not_missed + counter_received-check + 1;
+ }
counter_received=check;
memcpy(&received_transparancy,&data[4],4);
memcpy(&received_passivity,&data[8],4);
memcpy(&input,&data[12],4);
memcpy(&received_package,&data[16],4);
- led3=!led3;
- counter_not_missed ++;
- percentage_received = (float) counter_not_missed/counter_received;
-
- //pc.printf("data:%i,%i,%i,%f,%f\r\n",counter_received,received_transparancy,received_passivity,input,received_package);
+ percentage_received = (float) counter_not_missed/100;
+ if(percentage_received<0.90) {
+ led3=1;
+ } else {
+ led3=0;
}
+ }
}
}
}
@@ -434,109 +413,92 @@
osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);
-int main(){
+int main()
+{
osThreadCreate(osThread(receiveUDP), NULL);
-
+
inet_eth();
inet_USB();
-
+
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.rise(&updateTransparency);
Button2.rise(&updatePassivity);
-
+
t.start();
-
- while(true){
- if(controller_check==1){
+
+ while(true) {
+ if(controller_check==1) {
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
-
- if(identity==0)
- {
+
+ if(identity==0) {
transparancy = current_transparancy;
passivity = current_passivity;
- if(transparancy==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;
- generateOutput(ref_angle);
- }
- else if(transparancy==2)
- {
+ generateOutput(ref_angle);
+ } 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;
- generateOutput(force);
- }
- else if(transparancy==3)
- {
+ generateOutput(force);
+ } 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;
- generateOutput(ref_angle);
- }
- }
- else if(identity == 1)
- {
- if(transparancy==1)
- {
+ generateOutput(ref_angle);
+ }
+ } 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;
- generateOutput(ref_angle);
- }
- else if(transparancy==2)
- {
+ generateOutput(ref_angle);
+ } 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;
- generateOutput(ref_angle);
- }
- else if(transparancy==3)
- {
+ control_torque = torque_tlc;
+ generateOutput(ref_angle);
+ } 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;
- generateOutput(force);
+ control_torque = torque_tlc;
+ generateOutput(force);
}
}
- if(counter%2 >0)
-
- //pc.printf("force:%f, refpos:%f, pos:%f, tank:%f\r\n",force, ref_angle, angle, tank);
- //pc.printf("received input: %i\r\n",received_input);
- pc.printf("ticks: %i\r\n",encoderPos);
socket.sendTo(counterpart, output, sizeof(output));
socket.sendTo(counterpart, output, sizeof(output));
counter ++;
@@ -547,10 +509,10 @@
led2=0;
else
led2=1;
-
+
controller_check = 0;
debug = 0;
- }
+ }
osDelay(0.1); //this might be the most ugliest piece of code that somehow still works. BK
- }
-}
\ No newline at end of file
+ }
+}
\ No newline at end of file