Control a robot over the internet using UDP and a Ethernet interface.

Dependencies:   EthernetInterface Motor TextLCD mbed-rtos mbed Socket lwip-eth lwip-sys lwip

main.cpp

Committer:
apatel336
Date:
2013-10-17
Revision:
0:1496281373a5

File content as of revision 0:1496281373a5:

#include "mbed.h"
#include "math.h"
#include "TextLCD.h"
#include "rtos.h"
#include "time.h"
#include "Motor.h"
#include "EthernetInterface.h"
Mutex lock;

EthernetInterface eth;
Serial pc(USBTX, USBRX);
Motor m1(p21, p7, p8);
Motor m2(p22, p5, p6); // pwm, fwd, rev
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led4(LED4);
DigitalOut led3(LED3);
TextLCD lcd(p30, p29, p28, p27, p26, p25); // rs, e, d4-d7


float left_motor = 0.0;
float right_motor = 0.0;
void ether_f(void const *args);
void motor_f();
void time_disp(void const *args);
int main()
{
    Thread t1(time_disp);
    Thread t2(ether_f);
    while(1) {
        motor_f();
        Thread::wait(250);
    }
}

void time_disp(void const *args)
{
    while(true) {
        time_t temp = time(NULL);
        lock.lock();
        lcd.cls();
        lcd.printf("%s\n", ctime(&temp));
        lock.unlock();
        Thread::wait(4000);

        lock.lock();
        lcd.cls();
        lcd.printf("%s\n", eth.getIPAddress());
        lock.unlock();
        Thread::wait(4000);
    }
}

void motor_f()
{
    lock.lock();
    lcd.cls();
    lcd.printf("Testing Motor\n");
    lock.unlock();
    while (1) {
        m2.speed(left_motor);
        m1.speed(right_motor);
    }
}

void ether_f(void const *args)
{
    pc.printf("Testing Internet Connection!\n");
    led1 = 1;
    eth.init(); //Use DHCP
    Thread::wait(1000);
    pc.printf("MAC: %s\n", eth.getMACAddress());
    led2 = 1;
    eth.connect(30000);
    led1 = !led1;
    led2 = !led2;
    pc.printf("IP: %s\n", eth.getIPAddress());
    /* Code to Update RTC on mbed */
    /*
    UDPSocket sock;
    sock.init();

    Endpoint nist;
    nist.set_address("time-a.nist.gov", 37);

    char out_buffer[] = "plop"; // Does not matter
    sock.sendTo(nist, out_buffer, sizeof(out_buffer));

    char in_buffer[4];
    int n = sock.receiveFrom(nist, in_buffer, sizeof(in_buffer));

    unsigned int timeRes = ntohl( *((unsigned int*)in_buffer));
    set_time(timeRes - 2208988800U - 4*3600);
    pc.printf("Received %d bytes from server %s on port %d: %u seconds since 1/01/1900 00:00 GMT\n", n, nist.get_address(), nist.get_port(), timeRes);
    time_t temp = time(NULL);
    lcd.cls();
    lcd.printf("%s\n", ctime(&temp));
    sock.close();
    */
    UDPSocket server;
    server.bind(7);

    Endpoint client;
    char buffer[256];
    while (true) {
        int n = server.receiveFrom(client, buffer, sizeof(buffer));
        lock.lock();
        lcd.cls();
        lcd.printf("Received from:\n %s", client.get_address());
        lock.unlock();
        buffer[5] = NULL;
        pc.printf("%s\n", buffer);
        if (buffer[0] == '1') led1 = 1;
        else led1 = 0;
        if (buffer[1] == '1') led2 = 1;
        else led2 = 0;
        if (buffer[2] == '1') led3 = 1;
        else led3 = 0;
        if (buffer[3] == '1') led4 = 1;
        else led4 = 0;

        if (buffer[4] == '+') {
            left_motor += 0.1;
            if (left_motor > 1) left_motor = 1;
        } else if (buffer[4] == '-') {
            left_motor -= 0.1;
            if (left_motor < -1) left_motor = -1;
        } else if (buffer[4] == 'B') {
            left_motor = 0;
        } else if (buffer[4] == 'F') {
            if (left_motor < 0) left_motor = -1;
            else left_motor = 1;
        }


        if (buffer[4] == '+') {
            right_motor += 0.1;
            if (right_motor > 1) right_motor = 1;
        } else if (buffer[4] == '-') {
            right_motor -= 0.1;
            if (right_motor < -1) right_motor = -1;
        } else if (buffer[4] == 'B') {
            right_motor = 0;
        } else if (buffer[4] == 'F') {
            if (right_motor < 0) right_motor = -1;
            else right_motor = 1;
        }

        if (buffer[4] == 'L') {
            right_motor += 0.05;
            left_motor -= 0.05;
            if (right_motor > 1) right_motor = 1;
            if (left_motor < -1) left_motor = -1;
        }

        if (buffer[4] == 'R') {
            right_motor -= 0.05;
            left_motor += 0.05;
            if (left_motor > 1) left_motor = 1;
            if (right_motor < -1) right_motor = -1;
        }
        if (buffer[4] == 'S') {
            float temp = (right_motor + left_motor)/2;
            right_motor = temp;
            left_motor = temp;
        }
    }
    eth.disconnect();

}