Bachelor Assignment for delayed teleoperating systems

Dependencies:   EthernetInterface FastPWM mbed-rtos mbed MODSERIAL

main.cpp

Committer:
darth_bachious
Date:
2018-05-07
Revision:
0:2f89dec3e2ab
Child:
1:853939e38acd

File content as of revision 0:2f89dec3e2ab:

#include "mbed.h"
#include "EthernetInterface.h"
#include "rtos.h"
#include "QEI.h"


//network config
static const char* server_ip = "192.168.2.28";
static const int port = 865;

//declaration of interfaces

DigitalOut led(LED_GREEN); 
DigitalOut led2(LED_RED);
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
Endpoint counterpart;   //The actual other side, this is where the information should go to
Ticker mainloop;
Ticker sensor;

QEI M1(D3,D2,NC,1024);

//variables 
char data[10]= {""};
int size;
char * var;
char output[10] = {""};
float input1 = 0.0;
float input2 = 0.0;
bool main_loop_check = 0;
const float looptime  = 1.0/50; //50Hz
const float sensortime = 1.0/1000; //1 KHz
float angle = 0.0;



const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
const float RadsPerCount = (2 * PI)/(1024*10);
int frequency_pwm = 10000;




//functions to be used
void init_eth(void);
void inet_USB(void);
void end_eth(void);
void mainlooptrigger(void);
void SensorUpdate(void);
int main(void);

void inet_eth(){
    eth.init();
    eth.connect();
    
    socket.bind(port);
    counterpart.set_address(server_ip,port);
    }

void inet_USB(){
    pc.baud(9600);
    }
    

void end_eth(){
    socket.close();
    eth.disconnect();
    }

void mainlooptrigger()
    {
    main_loop_check = 1;
    }
void SensorUpdate()
    {
    angle = M1.getPulses()*RadsPerCount;    
    }

void receiveUDP(void const *argument){
    while(true){
        size = socket.receiveFrom(client, data, sizeof(data));
        if(size > 0){
            data[size] = '\0';
            pc.printf("data: %s \n",data);
            var = strtok(data,",;- ");
            input1 = atof(var);
            var = strtok(NULL,",;- ");
            input2 = atof(var);
            }
        }
    }

osThreadDef(receiveUDP, osPriorityNormal, DEFAULT_STACK_SIZE);

int main(){
    //inet_eth();
    inet_USB();
    
    osThreadCreate(osThread(receiveUDP), NULL);
    led2=1;
    led=1;
    mainloop.attach(&mainlooptrigger,looptime);
    sensor.attach(&SensorUpdate,sensortime);
    
    while(true){
                
        if(main_loop_check==1){
            //pc.printf("\nSERVER - Server IP Address is %s\r\n", eth.getIPAddress());
            //if(input1 > 0.0){
            //sprintf(output, "%f",input1);
            //pc.printf("sending answer to: %s",client.get_address());
            //pc.printf("\nSERVER - Server IP Address is %s\r\n", eth.getIPAddress());
            //socket.sendTo(counterpart, output, sizeof(output));
            //wait(0.001f);
            //}
            pc.printf("angle: %f\r\n", angle);
            led=!led;
            main_loop_check = 0;
            }
        osDelay(1);
        }  
}