Bas Koster / Mbed 2 deprecated Teleoperations

Bachelor Assignment for delayed teleoperating systems

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp@0:2f89dec3e2ab, 2018-05-07 (annotated)

Committer:
darth_bachious
Date:
Mon May 07 08:07:09 2018 +0000
Revision:
0:2f89dec3e2ab
Child:
1:853939e38acd
Base layer, from here work will be documented;

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 0:2f89dec3e2ab 5
darth_bachious 0:2f89dec3e2ab 6
darth_bachious 0:2f89dec3e2ab 7 //network config
darth_bachious 0:2f89dec3e2ab 8 static const char* server_ip = "192.168.2.28";
darth_bachious 0:2f89dec3e2ab 9 static const int port = 865;
darth_bachious 0:2f89dec3e2ab 10
darth_bachious 0:2f89dec3e2ab 11 //declaration of interfaces
darth_bachious 0:2f89dec3e2ab 12
darth_bachious 0:2f89dec3e2ab 13 DigitalOut led(LED_GREEN);
darth_bachious 0:2f89dec3e2ab 14 DigitalOut led2(LED_RED);
darth_bachious 0:2f89dec3e2ab 15 EthernetInterface eth; //network
darth_bachious 0:2f89dec3e2ab 16 Serial pc(USBTX, USBRX);//create PC interface
darth_bachious 0:2f89dec3e2ab 17 UDPSocket socket; //socket to receive data on
darth_bachious 0:2f89dec3e2ab 18 Endpoint client; //The virtual other side, not to send actual information to
darth_bachious 0:2f89dec3e2ab 19 Endpoint counterpart; //The actual other side, this is where the information should go to
darth_bachious 0:2f89dec3e2ab 20 Ticker mainloop;
darth_bachious 0:2f89dec3e2ab 21 Ticker sensor;
darth_bachious 0:2f89dec3e2ab 22
darth_bachious 0:2f89dec3e2ab 23 QEI M1(D3,D2,NC,1024);
darth_bachious 0:2f89dec3e2ab 24
darth_bachious 0:2f89dec3e2ab 25 //variables
darth_bachious 0:2f89dec3e2ab 26 char data[10]= {""};
darth_bachious 0:2f89dec3e2ab 27 int size;
darth_bachious 0:2f89dec3e2ab 28 char * var;
darth_bachious 0:2f89dec3e2ab 29 char output[10] = {""};
darth_bachious 0:2f89dec3e2ab 30 float input1 = 0.0;
darth_bachious 0:2f89dec3e2ab 31 float input2 = 0.0;
darth_bachious 0:2f89dec3e2ab 32 bool main_loop_check = 0;
darth_bachious 0:2f89dec3e2ab 33 const float looptime = 1.0/50; //50Hz
darth_bachious 0:2f89dec3e2ab 34 const float sensortime = 1.0/1000; //1 KHz
darth_bachious 0:2f89dec3e2ab 35 float angle = 0.0;
darth_bachious 0:2f89dec3e2ab 36
darth_bachious 0:2f89dec3e2ab 37
darth_bachious 0:2f89dec3e2ab 38
darth_bachious 0:2f89dec3e2ab 39 const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
darth_bachious 0:2f89dec3e2ab 40 const float RadsPerCount = (2 * PI)/(1024*10);
darth_bachious 0:2f89dec3e2ab 41 int frequency_pwm = 10000;
darth_bachious 0:2f89dec3e2ab 42
darth_bachious 0:2f89dec3e2ab 43
darth_bachious 0:2f89dec3e2ab 44
darth_bachious 0:2f89dec3e2ab 45
darth_bachious 0:2f89dec3e2ab 46 //functions to be used
darth_bachious 0:2f89dec3e2ab 47 void init_eth(void);
darth_bachious 0:2f89dec3e2ab 48 void inet_USB(void);
darth_bachious 0:2f89dec3e2ab 49 void end_eth(void);
darth_bachious 0:2f89dec3e2ab 50 void mainlooptrigger(void);
darth_bachious 0:2f89dec3e2ab 51 void SensorUpdate(void);
darth_bachious 0:2f89dec3e2ab 52 int main(void);
darth_bachious 0:2f89dec3e2ab 53
darth_bachious 0:2f89dec3e2ab 54 void inet_eth(){
darth_bachious 0:2f89dec3e2ab 55 eth.init();
darth_bachious 0:2f89dec3e2ab 56 eth.connect();
darth_bachious 0:2f89dec3e2ab 57
darth_bachious 0:2f89dec3e2ab 58 socket.bind(port);
darth_bachious 0:2f89dec3e2ab 59 counterpart.set_address(server_ip,port);
darth_bachious 0:2f89dec3e2ab 60 }
darth_bachious 0:2f89dec3e2ab 61
darth_bachious 0:2f89dec3e2ab 62 void inet_USB(){
darth_bachious 0:2f89dec3e2ab 63 pc.baud(9600);
darth_bachious 0:2f89dec3e2ab 64 }
darth_bachious 0:2f89dec3e2ab 65
darth_bachious 0:2f89dec3e2ab 66
darth_bachious 0:2f89dec3e2ab 67 void end_eth(){
darth_bachious 0:2f89dec3e2ab 68 socket.close();
darth_bachious 0:2f89dec3e2ab 69 eth.disconnect();
darth_bachious 0:2f89dec3e2ab 70 }
darth_bachious 0:2f89dec3e2ab 71
darth_bachious 0:2f89dec3e2ab 72 void mainlooptrigger()
darth_bachious 0:2f89dec3e2ab 73 {
darth_bachious 0:2f89dec3e2ab 74 main_loop_check = 1;
darth_bachious 0:2f89dec3e2ab 75 }
darth_bachious 0:2f89dec3e2ab 76 void SensorUpdate()
darth_bachious 0:2f89dec3e2ab 77 {
darth_bachious 0:2f89dec3e2ab 78 angle = M1.getPulses()*RadsPerCount;
darth_bachious 0:2f89dec3e2ab 79 }
darth_bachious 0:2f89dec3e2ab 80
darth_bachious 0:2f89dec3e2ab 81 void receiveUDP(void const *argument){
darth_bachious 0:2f89dec3e2ab 82 while(true){
darth_bachious 0:2f89dec3e2ab 83 size = socket.receiveFrom(client, data, sizeof(data));
darth_bachious 0:2f89dec3e2ab 84 if(size > 0){
darth_bachious 0:2f89dec3e2ab 85 data[size] = '\0';
darth_bachious 0:2f89dec3e2ab 86 pc.printf("data: %s \n",data);
darth_bachious 0:2f89dec3e2ab 87 var = strtok(data,",;- ");
darth_bachious 0:2f89dec3e2ab 88 input1 = atof(var);
darth_bachious 0:2f89dec3e2ab 89 var = strtok(NULL,",;- ");
darth_bachious 0:2f89dec3e2ab 90 input2 = atof(var);
darth_bachious 0:2f89dec3e2ab 91 }
darth_bachious 0:2f89dec3e2ab 92 }
darth_bachious 0:2f89dec3e2ab 93 }
darth_bachious 0:2f89dec3e2ab 94
darth_bachious 0:2f89dec3e2ab 95 osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);
darth_bachious 0:2f89dec3e2ab 96
darth_bachious 0:2f89dec3e2ab 97 int main(){
darth_bachious 0:2f89dec3e2ab 98 //inet_eth();
darth_bachious 0:2f89dec3e2ab 99 inet_USB();
darth_bachious 0:2f89dec3e2ab 100
darth_bachious 0:2f89dec3e2ab 101 osThreadCreate(osThread(receiveUDP), NULL);
darth_bachious 0:2f89dec3e2ab 102 led2=1;
darth_bachious 0:2f89dec3e2ab 103 led=1;
darth_bachious 0:2f89dec3e2ab 104 mainloop.attach(&mainlooptrigger,looptime);
darth_bachious 0:2f89dec3e2ab 105 sensor.attach(&SensorUpdate,sensortime);
darth_bachious 0:2f89dec3e2ab 106
darth_bachious 0:2f89dec3e2ab 107 while(true){
darth_bachious 0:2f89dec3e2ab 108
darth_bachious 0:2f89dec3e2ab 109 if(main_loop_check==1){
darth_bachious 0:2f89dec3e2ab 110 //pc.printf("\nSERVER - Server IP Address is %s\r\n", eth.getIPAddress());
darth_bachious 0:2f89dec3e2ab 111 //if(input1 > 0.0){
darth_bachious 0:2f89dec3e2ab 112 //sprintf(output, "%f",input1);
darth_bachious 0:2f89dec3e2ab 113 //pc.printf("sending answer to: %s",client.get_address());
darth_bachious 0:2f89dec3e2ab 114 //pc.printf("\nSERVER - Server IP Address is %s\r\n", eth.getIPAddress());
darth_bachious 0:2f89dec3e2ab 115 //socket.sendTo(counterpart, output, sizeof(output));
darth_bachious 0:2f89dec3e2ab 116 //wait(0.001f);
darth_bachious 0:2f89dec3e2ab 117 //}
darth_bachious 0:2f89dec3e2ab 118 pc.printf("angle: %f\r\n", angle);
darth_bachious 0:2f89dec3e2ab 119 led=!led;
darth_bachious 0:2f89dec3e2ab 120 main_loop_check = 0;
darth_bachious 0:2f89dec3e2ab 121 }
darth_bachious 0:2f89dec3e2ab 122 osDelay(1);
darth_bachious 0:2f89dec3e2ab 123 }
darth_bachious 0:2f89dec3e2ab 124 }