Bas Koster / Mbed 2 deprecated Teleoperations

Bachelor Assignment for delayed teleoperating systems

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp@2:c27b0654cffd, 2018-05-08 (annotated)

Committer:
darth_bachious
Date:
Tue May 08 08:49:24 2018 +0000
Revision:
2:c27b0654cffd
Parent:
1:853939e38acd
Child:
3:376fccdc7cd6
Two-way communications, now able to generate an torque based on displacement;

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 0:2f89dec3e2ab 4 #include "QEI.h"
darth_bachious 2:c27b0654cffd 5 #include <vector>
darth_bachious 2:c27b0654cffd 6 #include <cmath>
darth_bachious 0:2f89dec3e2ab 7
darth_bachious 1:853939e38acd 8 //Master or Slave? 1=Master, 0=Slave
darth_bachious 2:c27b0654cffd 9 static const int identity = 0;
darth_bachious 0:2f89dec3e2ab 10
darth_bachious 0:2f89dec3e2ab 11 //network config
darth_bachious 1:853939e38acd 12 static const char* master_ip = "192.168.1.101";
darth_bachious 1:853939e38acd 13 static const char* slave_ip = "192.168.1.102";
darth_bachious 1:853939e38acd 14 static const char* MASK = "255.255.255.0";
darth_bachious 1:853939e38acd 15 static const char* GATEWAY = "192.168.1.1";
darth_bachious 0:2f89dec3e2ab 16 static const int port = 865;
darth_bachious 0:2f89dec3e2ab 17
darth_bachious 0:2f89dec3e2ab 18 //declaration of interfaces
darth_bachious 0:2f89dec3e2ab 19
darth_bachious 0:2f89dec3e2ab 20 DigitalOut led(LED_GREEN);
darth_bachious 0:2f89dec3e2ab 21 DigitalOut led2(LED_RED);
darth_bachious 0:2f89dec3e2ab 22 EthernetInterface eth; //network
darth_bachious 0:2f89dec3e2ab 23 Serial pc(USBTX, USBRX);//create PC interface
darth_bachious 0:2f89dec3e2ab 24 UDPSocket socket; //socket to receive data on
darth_bachious 0:2f89dec3e2ab 25 Endpoint client; //The virtual other side, not to send actual information to
darth_bachious 0:2f89dec3e2ab 26 Endpoint counterpart; //The actual other side, this is where the information should go to
darth_bachious 0:2f89dec3e2ab 27 Ticker mainloop;
darth_bachious 0:2f89dec3e2ab 28
darth_bachious 1:853939e38acd 29 QEI M1(D3,D2,NC,1024,QEI::X4_ENCODING);
darth_bachious 0:2f89dec3e2ab 30
darth_bachious 0:2f89dec3e2ab 31 //variables
darth_bachious 1:853939e38acd 32 char data[30]= {""};
darth_bachious 0:2f89dec3e2ab 33 int size;
darth_bachious 1:853939e38acd 34 int counter = 1;
darth_bachious 1:853939e38acd 35 int counter_received = 1;
darth_bachious 0:2f89dec3e2ab 36 char * var;
darth_bachious 1:853939e38acd 37 char output[30] = {""};
darth_bachious 2:c27b0654cffd 38 float input = 0.0;
darth_bachious 2:c27b0654cffd 39 float angle = 0.0;
darth_bachious 2:c27b0654cffd 40 float prev_angle = 0.0;
darth_bachious 2:c27b0654cffd 41 float torqueTL = 0.0;
darth_bachious 2:c27b0654cffd 42
darth_bachious 0:2f89dec3e2ab 43 bool main_loop_check = 0;
darth_bachious 0:2f89dec3e2ab 44 const float looptime = 1.0/50; //50Hz
darth_bachious 2:c27b0654cffd 45
darth_bachious 2:c27b0654cffd 46 const float Ktl = 20;
darth_bachious 2:c27b0654cffd 47 const float Dtl=0.5;
darth_bachious 0:2f89dec3e2ab 48
darth_bachious 0:2f89dec3e2ab 49 const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
darth_bachious 2:c27b0654cffd 50 const float RadsPerCount = (2 * PI)/(1024*10);
darth_bachious 2:c27b0654cffd 51
darth_bachious 2:c27b0654cffd 52
darth_bachious 0:2f89dec3e2ab 53 int frequency_pwm = 10000;
darth_bachious 0:2f89dec3e2ab 54
darth_bachious 2:c27b0654cffd 55 float timedelay = 0.20; //SECONDS
darth_bachious 2:c27b0654cffd 56 std::vector<float> delayArrayINPUT(ceil(timedelay/looptime),0.0);
darth_bachious 2:c27b0654cffd 57
darth_bachious 0:2f89dec3e2ab 58
darth_bachious 0:2f89dec3e2ab 59 void inet_eth(){
darth_bachious 1:853939e38acd 60 if(identity==1)
darth_bachious 1:853939e38acd 61 {
darth_bachious 1:853939e38acd 62 eth.init(master_ip, MASK,GATEWAY);
darth_bachious 1:853939e38acd 63 eth.connect();
darth_bachious 0:2f89dec3e2ab 64
darth_bachious 1:853939e38acd 65 socket.bind(port);
darth_bachious 1:853939e38acd 66 counterpart.set_address(slave_ip,port);
darth_bachious 1:853939e38acd 67 }
darth_bachious 1:853939e38acd 68 else if(identity==0)
darth_bachious 1:853939e38acd 69 {
darth_bachious 1:853939e38acd 70 eth.init(slave_ip, MASK,GATEWAY);
darth_bachious 1:853939e38acd 71 eth.connect();
darth_bachious 1:853939e38acd 72
darth_bachious 1:853939e38acd 73 socket.bind(port);
darth_bachious 1:853939e38acd 74 counterpart.set_address(master_ip,port);
darth_bachious 1:853939e38acd 75 }
darth_bachious 1:853939e38acd 76
darth_bachious 0:2f89dec3e2ab 77 }
darth_bachious 0:2f89dec3e2ab 78
darth_bachious 0:2f89dec3e2ab 79 void inet_USB(){
darth_bachious 2:c27b0654cffd 80 pc.baud(115200);
darth_bachious 0:2f89dec3e2ab 81 }
darth_bachious 0:2f89dec3e2ab 82
darth_bachious 0:2f89dec3e2ab 83
darth_bachious 0:2f89dec3e2ab 84 void end_eth(){
darth_bachious 0:2f89dec3e2ab 85 socket.close();
darth_bachious 0:2f89dec3e2ab 86 eth.disconnect();
darth_bachious 0:2f89dec3e2ab 87 }
darth_bachious 0:2f89dec3e2ab 88
darth_bachious 0:2f89dec3e2ab 89 void mainlooptrigger()
darth_bachious 0:2f89dec3e2ab 90 {
darth_bachious 0:2f89dec3e2ab 91 main_loop_check = 1;
darth_bachious 0:2f89dec3e2ab 92 }
darth_bachious 2:c27b0654cffd 93
darth_bachious 2:c27b0654cffd 94 float update_delay(std::vector<float>&array, float new_value)
darth_bachious 2:c27b0654cffd 95 {
darth_bachious 2:c27b0654cffd 96 float return_value = array[1];
darth_bachious 2:c27b0654cffd 97 for (int i=0; i<array.size()-1; ++i)
darth_bachious 2:c27b0654cffd 98 {
darth_bachious 2:c27b0654cffd 99 array[i]=array[i+1];
darth_bachious 2:c27b0654cffd 100 }
darth_bachious 2:c27b0654cffd 101 array.back() = new_value;
darth_bachious 2:c27b0654cffd 102 return return_value;
darth_bachious 2:c27b0654cffd 103 }
darth_bachious 0:2f89dec3e2ab 104
darth_bachious 0:2f89dec3e2ab 105 void receiveUDP(void const *argument){
darth_bachious 0:2f89dec3e2ab 106 while(true){
darth_bachious 0:2f89dec3e2ab 107 size = socket.receiveFrom(client, data, sizeof(data));
darth_bachious 0:2f89dec3e2ab 108 if(size > 0){
darth_bachious 0:2f89dec3e2ab 109 data[size] = '\0';
darth_bachious 2:c27b0654cffd 110 if(size>5) //first check, an minimum amount of data must have arrived
darth_bachious 2:c27b0654cffd 111 {
darth_bachious 2:c27b0654cffd 112 var = strtok(data,"; ");
darth_bachious 2:c27b0654cffd 113 if(counter_received < atof(var)) //second check, data must be newer
darth_bachious 2:c27b0654cffd 114 {
darth_bachious 2:c27b0654cffd 115 counter_received = atof(var);
darth_bachious 2:c27b0654cffd 116 var = strtok(NULL,"; ");
darth_bachious 2:c27b0654cffd 117 input = atof(var);
darth_bachious 2:c27b0654cffd 118 //pc.printf("data: %s \n\r",data);
darth_bachious 2:c27b0654cffd 119 }
darth_bachious 2:c27b0654cffd 120 }
darth_bachious 1:853939e38acd 121 //pc.printf("data: %s \n\r",data);
darth_bachious 0:2f89dec3e2ab 122 }
darth_bachious 0:2f89dec3e2ab 123 }
darth_bachious 0:2f89dec3e2ab 124 }
darth_bachious 0:2f89dec3e2ab 125
darth_bachious 0:2f89dec3e2ab 126 osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);
darth_bachious 0:2f89dec3e2ab 127
darth_bachious 0:2f89dec3e2ab 128 int main(){
darth_bachious 1:853939e38acd 129 inet_eth();
darth_bachious 0:2f89dec3e2ab 130 inet_USB();
darth_bachious 0:2f89dec3e2ab 131
darth_bachious 0:2f89dec3e2ab 132 osThreadCreate(osThread(receiveUDP), NULL);
darth_bachious 0:2f89dec3e2ab 133 led2=1;
darth_bachious 0:2f89dec3e2ab 134 led=1;
darth_bachious 0:2f89dec3e2ab 135 mainloop.attach(&mainlooptrigger,looptime);
darth_bachious 0:2f89dec3e2ab 136
darth_bachious 0:2f89dec3e2ab 137 while(true){
darth_bachious 2:c27b0654cffd 138
darth_bachious 0:2f89dec3e2ab 139 if(main_loop_check==1){
darth_bachious 1:853939e38acd 140 angle = M1.getPulses()*RadsPerCount;
darth_bachious 2:c27b0654cffd 141 float reference = update_delay(delayArrayINPUT,input);
darth_bachious 2:c27b0654cffd 142
darth_bachious 2:c27b0654cffd 143 torqueTL = Ktl*(reference-angle) - Dtl * (angle-prev_angle) / looptime;
darth_bachious 2:c27b0654cffd 144
darth_bachious 2:c27b0654cffd 145 pc.printf("Torque: %f\r\n",torqueTL);
darth_bachious 1:853939e38acd 146 sprintf(output, "%i;%f",counter,angle);
darth_bachious 1:853939e38acd 147 socket.sendTo(counterpart, output, sizeof(output));
darth_bachious 1:853939e38acd 148 counter = counter + 1;
darth_bachious 0:2f89dec3e2ab 149 led=!led;
darth_bachious 0:2f89dec3e2ab 150 main_loop_check = 0;
darth_bachious 2:c27b0654cffd 151 prev_angle = angle;
darth_bachious 0:2f89dec3e2ab 152 }
darth_bachious 0:2f89dec3e2ab 153 osDelay(1);
darth_bachious 0:2f89dec3e2ab 154 }
darth_bachious 0:2f89dec3e2ab 155 }