Bas Koster / Mbed 2 deprecated Teleoperations

Bachelor Assignment for delayed teleoperating systems

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp@3:376fccdc7cd6, 2018-05-08 (annotated)

Committer:
darth_bachious
Date:
Tue May 08 13:14:18 2018 +0000
Revision:
3:376fccdc7cd6
Parent:
2:c27b0654cffd
Child:
4:610b5051182a
Functional POS-POS system, without passivity

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 3:376fccdc7cd6 28
darth_bachious 3:376fccdc7cd6 29 InterruptIn EncoderA(D2);
darth_bachious 3:376fccdc7cd6 30 DigitalIn EncoderB(D3);
darth_bachious 3:376fccdc7cd6 31
darth_bachious 3:376fccdc7cd6 32 DigitalOut M1_DIR(D4);
darth_bachious 3:376fccdc7cd6 33 PwmOut M1_pwm(D5);
darth_bachious 0:2f89dec3e2ab 34
darth_bachious 0:2f89dec3e2ab 35 //variables
darth_bachious 1:853939e38acd 36 char data[30]= {""};
darth_bachious 0:2f89dec3e2ab 37 int size;
darth_bachious 1:853939e38acd 38 int counter = 1;
darth_bachious 1:853939e38acd 39 int counter_received = 1;
darth_bachious 0:2f89dec3e2ab 40 char * var;
darth_bachious 1:853939e38acd 41 char output[30] = {""};
darth_bachious 2:c27b0654cffd 42 float input = 0.0;
darth_bachious 2:c27b0654cffd 43 float angle = 0.0;
darth_bachious 2:c27b0654cffd 44 float prev_angle = 0.0;
darth_bachious 3:376fccdc7cd6 45
darth_bachious 3:376fccdc7cd6 46 int encoderPos = 0;
darth_bachious 2:c27b0654cffd 47
darth_bachious 0:2f89dec3e2ab 48 bool main_loop_check = 0;
darth_bachious 0:2f89dec3e2ab 49 const float looptime = 1.0/50; //50Hz
darth_bachious 2:c27b0654cffd 50
darth_bachious 3:376fccdc7cd6 51 const float Ktl = 4;
darth_bachious 3:376fccdc7cd6 52 const float Dtl=0.1;
darth_bachious 0:2f89dec3e2ab 53
darth_bachious 0:2f89dec3e2ab 54 const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
darth_bachious 2:c27b0654cffd 55 const float RadsPerCount = (2 * PI)/(1024*10);
darth_bachious 2:c27b0654cffd 56
darth_bachious 3:376fccdc7cd6 57 int frequency_pwm = 20000;
darth_bachious 0:2f89dec3e2ab 58
darth_bachious 2:c27b0654cffd 59 float timedelay = 0.20; //SECONDS
darth_bachious 2:c27b0654cffd 60 std::vector<float> delayArrayINPUT(ceil(timedelay/looptime),0.0);
darth_bachious 2:c27b0654cffd 61
darth_bachious 3:376fccdc7cd6 62 //FUNCTIONS START HERE
darth_bachious 3:376fccdc7cd6 63
darth_bachious 3:376fccdc7cd6 64 void encoderFunctionA()
darth_bachious 3:376fccdc7cd6 65 {
darth_bachious 3:376fccdc7cd6 66 if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
darth_bachious 3:376fccdc7cd6 67 encoderPos++;
darth_bachious 3:376fccdc7cd6 68 } else {
darth_bachious 3:376fccdc7cd6 69 encoderPos--;
darth_bachious 3:376fccdc7cd6 70 }
darth_bachious 3:376fccdc7cd6 71 }
darth_bachious 3:376fccdc7cd6 72
darth_bachious 0:2f89dec3e2ab 73
darth_bachious 0:2f89dec3e2ab 74 void inet_eth(){
darth_bachious 1:853939e38acd 75 if(identity==1)
darth_bachious 1:853939e38acd 76 {
darth_bachious 1:853939e38acd 77 eth.init(master_ip, MASK,GATEWAY);
darth_bachious 1:853939e38acd 78 eth.connect();
darth_bachious 0:2f89dec3e2ab 79
darth_bachious 1:853939e38acd 80 socket.bind(port);
darth_bachious 1:853939e38acd 81 counterpart.set_address(slave_ip,port);
darth_bachious 1:853939e38acd 82 }
darth_bachious 1:853939e38acd 83 else if(identity==0)
darth_bachious 1:853939e38acd 84 {
darth_bachious 1:853939e38acd 85 eth.init(slave_ip, MASK,GATEWAY);
darth_bachious 1:853939e38acd 86 eth.connect();
darth_bachious 1:853939e38acd 87
darth_bachious 1:853939e38acd 88 socket.bind(port);
darth_bachious 1:853939e38acd 89 counterpart.set_address(master_ip,port);
darth_bachious 1:853939e38acd 90 }
darth_bachious 1:853939e38acd 91
darth_bachious 0:2f89dec3e2ab 92 }
darth_bachious 0:2f89dec3e2ab 93
darth_bachious 0:2f89dec3e2ab 94 void inet_USB(){
darth_bachious 2:c27b0654cffd 95 pc.baud(115200);
darth_bachious 0:2f89dec3e2ab 96 }
darth_bachious 0:2f89dec3e2ab 97
darth_bachious 0:2f89dec3e2ab 98
darth_bachious 0:2f89dec3e2ab 99 void end_eth(){
darth_bachious 0:2f89dec3e2ab 100 socket.close();
darth_bachious 0:2f89dec3e2ab 101 eth.disconnect();
darth_bachious 0:2f89dec3e2ab 102 }
darth_bachious 0:2f89dec3e2ab 103
darth_bachious 0:2f89dec3e2ab 104 void mainlooptrigger()
darth_bachious 0:2f89dec3e2ab 105 {
darth_bachious 0:2f89dec3e2ab 106 main_loop_check = 1;
darth_bachious 0:2f89dec3e2ab 107 }
darth_bachious 2:c27b0654cffd 108
darth_bachious 2:c27b0654cffd 109 float update_delay(std::vector<float>&array, float new_value)
darth_bachious 2:c27b0654cffd 110 {
darth_bachious 3:376fccdc7cd6 111 float return_value = array[1];
darth_bachious 3:376fccdc7cd6 112 for (int i=0; i<array.size()-1; ++i)
darth_bachious 3:376fccdc7cd6 113 {
darth_bachious 3:376fccdc7cd6 114 array[i]=array[i+1];
darth_bachious 3:376fccdc7cd6 115 }
darth_bachious 3:376fccdc7cd6 116 array.back() = new_value;
darth_bachious 3:376fccdc7cd6 117 return return_value;
darth_bachious 3:376fccdc7cd6 118 }
darth_bachious 3:376fccdc7cd6 119
darth_bachious 3:376fccdc7cd6 120 void limit(float &x, float lower, float upper)
darth_bachious 3:376fccdc7cd6 121 {
darth_bachious 3:376fccdc7cd6 122 if (x > upper)
darth_bachious 3:376fccdc7cd6 123 x = upper;
darth_bachious 3:376fccdc7cd6 124 if (x < lower)
darth_bachious 3:376fccdc7cd6 125 x = lower;
darth_bachious 3:376fccdc7cd6 126 }
darth_bachious 3:376fccdc7cd6 127
darth_bachious 3:376fccdc7cd6 128 void motor_update(float angle, float &torque) //angle required to safeguard it from crashing into its stops
darth_bachious 3:376fccdc7cd6 129 {
darth_bachious 3:376fccdc7cd6 130 //if((angle>PI/4)&&(torque>0.0))
darth_bachious 3:376fccdc7cd6 131 // {
darth_bachious 3:376fccdc7cd6 132 // torque = 0;
darth_bachious 3:376fccdc7cd6 133 // }
darth_bachious 3:376fccdc7cd6 134 // if((angle<-PI/4)&&(torque<0.0))
darth_bachious 3:376fccdc7cd6 135 // {
darth_bachious 3:376fccdc7cd6 136 // torque = 0;
darth_bachious 3:376fccdc7cd6 137 // }
darth_bachious 3:376fccdc7cd6 138
darth_bachious 3:376fccdc7cd6 139 float PWM = torque*0.58; //converting torque to PWM.
darth_bachious 3:376fccdc7cd6 140 pc.printf("PWM: %f\r\n",PWM);
darth_bachious 3:376fccdc7cd6 141 limit(PWM,-1.0f,1.0f);
darth_bachious 3:376fccdc7cd6 142 //pc.printf("PWM: %f\r\n",PWM);
darth_bachious 3:376fccdc7cd6 143 if(PWM >= 0.0f)
darth_bachious 2:c27b0654cffd 144 {
darth_bachious 3:376fccdc7cd6 145 M1_DIR = false;
darth_bachious 3:376fccdc7cd6 146 M1_pwm = PWM;
darth_bachious 3:376fccdc7cd6 147 } else {
darth_bachious 3:376fccdc7cd6 148 M1_DIR = true;
darth_bachious 3:376fccdc7cd6 149 M1_pwm = -PWM;
darth_bachious 2:c27b0654cffd 150 }
darth_bachious 3:376fccdc7cd6 151
darth_bachious 3:376fccdc7cd6 152
darth_bachious 2:c27b0654cffd 153 }
darth_bachious 0:2f89dec3e2ab 154
darth_bachious 0:2f89dec3e2ab 155 void receiveUDP(void const *argument){
darth_bachious 0:2f89dec3e2ab 156 while(true){
darth_bachious 0:2f89dec3e2ab 157 size = socket.receiveFrom(client, data, sizeof(data));
darth_bachious 0:2f89dec3e2ab 158 if(size > 0){
darth_bachious 0:2f89dec3e2ab 159 data[size] = '\0';
darth_bachious 3:376fccdc7cd6 160
darth_bachious 2:c27b0654cffd 161 if(size>5) //first check, an minimum amount of data must have arrived
darth_bachious 2:c27b0654cffd 162 {
darth_bachious 2:c27b0654cffd 163 var = strtok(data,"; ");
darth_bachious 2:c27b0654cffd 164 if(counter_received < atof(var)) //second check, data must be newer
darth_bachious 2:c27b0654cffd 165 {
darth_bachious 2:c27b0654cffd 166 counter_received = atof(var);
darth_bachious 2:c27b0654cffd 167 var = strtok(NULL,"; ");
darth_bachious 2:c27b0654cffd 168 input = atof(var);
darth_bachious 3:376fccdc7cd6 169 //pc.printf("input: %f \n\r",input);
darth_bachious 3:376fccdc7cd6 170 //MORE CHECKS NEED TO BE IMPLEMENTED
darth_bachious 2:c27b0654cffd 171 }
darth_bachious 2:c27b0654cffd 172 }
darth_bachious 0:2f89dec3e2ab 173 }
darth_bachious 0:2f89dec3e2ab 174 }
darth_bachious 0:2f89dec3e2ab 175 }
darth_bachious 0:2f89dec3e2ab 176
darth_bachious 0:2f89dec3e2ab 177 osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);
darth_bachious 0:2f89dec3e2ab 178
darth_bachious 0:2f89dec3e2ab 179 int main(){
darth_bachious 1:853939e38acd 180 inet_eth();
darth_bachious 0:2f89dec3e2ab 181 inet_USB();
darth_bachious 0:2f89dec3e2ab 182
darth_bachious 0:2f89dec3e2ab 183 osThreadCreate(osThread(receiveUDP), NULL);
darth_bachious 0:2f89dec3e2ab 184 led2=1;
darth_bachious 0:2f89dec3e2ab 185 led=1;
darth_bachious 3:376fccdc7cd6 186
darth_bachious 0:2f89dec3e2ab 187 mainloop.attach(&mainlooptrigger,looptime);
darth_bachious 0:2f89dec3e2ab 188
darth_bachious 3:376fccdc7cd6 189 M1_pwm.period(1.0/frequency_pwm);
darth_bachious 3:376fccdc7cd6 190
darth_bachious 3:376fccdc7cd6 191 EncoderA.rise(&encoderFunctionA);
darth_bachious 3:376fccdc7cd6 192
darth_bachious 0:2f89dec3e2ab 193 while(true){
darth_bachious 0:2f89dec3e2ab 194 if(main_loop_check==1){
darth_bachious 3:376fccdc7cd6 195 angle = encoderPos*RadsPerCount;
darth_bachious 3:376fccdc7cd6 196 //pc.printf("Angle: %f\r\n",angle);
darth_bachious 2:c27b0654cffd 197 float reference = update_delay(delayArrayINPUT,input);
darth_bachious 2:c27b0654cffd 198
darth_bachious 3:376fccdc7cd6 199 float torqueTL = Ktl*(reference-angle)- Dtl * (angle-prev_angle) / looptime;
darth_bachious 2:c27b0654cffd 200
darth_bachious 3:376fccdc7cd6 201 motor_update(angle,torqueTL);
darth_bachious 3:376fccdc7cd6 202
darth_bachious 3:376fccdc7cd6 203 //pc.printf("Torque: %f\r\n",torqueTL);
darth_bachious 1:853939e38acd 204 sprintf(output, "%i;%f",counter,angle);
darth_bachious 1:853939e38acd 205 socket.sendTo(counterpart, output, sizeof(output));
darth_bachious 3:376fccdc7cd6 206 counter ++;
darth_bachious 0:2f89dec3e2ab 207 led=!led;
darth_bachious 0:2f89dec3e2ab 208 main_loop_check = 0;
darth_bachious 2:c27b0654cffd 209 prev_angle = angle;
darth_bachious 0:2f89dec3e2ab 210 }
darth_bachious 0:2f89dec3e2ab 211 osDelay(1);
darth_bachious 0:2f89dec3e2ab 212 }
darth_bachious 0:2f89dec3e2ab 213 }