File content as of revision 3:10fa3102c2d7:

#include "mbed.h"
#include "odrive.h"
#include "lwip-interface/EthernetInterface.h"
#include "comms.h"
#include <string>
#include "BufferedSerial.h"
DigitalOut led1(LED1);
DigitalOut homeGND(PF_5);
DigitalIn homeSwitchA(PA_3);
DigitalIn homeSwitchB(PC_0);
DigitalIn homeSwitchC(PC_3);
DigitalIn trigger(PF_3);

using std::string;
const int BROADCAST_PORT_T = 58080;
const int BROADCAST_PORT_R = 58081;
EthernetInterface eth;

BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX);


void transmit()
{
    UDPSocket socket(&eth);
    string out_buffer = "very important data";
    SocketAddress transmit("10.0.0.160", BROADCAST_PORT_T);
    fromRobot msg;
    msg.motor1.busVoltage = 69;
    while (true) {
        msg.motor1.busVoltage += 1;
        int ret = socket.sendto(transmit, &msg, sizeof(msg));
        //printf("sendto return: %d\n", ret);

        Thread::wait(100);
    }
}

void receive()
{
    SocketAddress receive;
    UDPSocket socket(&eth);
    int bind = socket.bind(BROADCAST_PORT_R);
    //printf("bind return: %d", bind);

    char buffer[256];
    while (true) {
        //printf("\nWait for packet...\n");
        int n = socket.recvfrom(&receive, buffer, sizeof(buffer));
        toRobot inmsg;
        memcpy(&inmsg, buffer, n);
        //buffer[n] = '\0';
        buffered_pc.printf("Count:%i, Motor1Vel:%f, Motor1Tor: %f \r\n",inmsg.count, inmsg.motor1.velocity,inmsg.motor1.torque);

        //Thread::wait(1);
    }
}

int getCountPos(BufferedSerial ser, int axis)
{

}

void runOdrive()
{
    BufferedSerial ODSerial0(PC_12,PD_2);
    ODSerial0.baud(115200);
    BufferedSerial ODSerial1(PD_5,PD_6);
    ODSerial1.baud(115200);
    ODrive OD0(ODSerial0);
    ODrive OD1(ODSerial1);
    int requested_state;
    requested_state = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL;
    OD0.run_state(0, requested_state, false); // don't wait
    OD1.run_state(0, requested_state, false); // don't wait
    OD1.run_state(1, requested_state, false); // don't wait
    Thread::wait(1000);
//   OD0.SetVelocity(0,-1000);
//    OD1.SetVelocity(1,1000);
    //OD1.SetVelocity(1,1000);
//    OD1.SetVelocity(0,1000);
//    OD1.SetVelocity(1,1000);
    
    bool Ahomed = false;
    int homeCountA = 0;
    int homeOffsetA = 0;
    while(!Ahomed) {
        OD1.SetPosition(1,homeOffsetA);
        Thread::wait(1);
        for(int i = 0 ; i < 99; i++) {
            if(homeSwitchA == false) {
                homeCountA++;
            } else {
                homeCountA = 0;
            }
            if(homeCountA > 100) {
                Ahomed = true;
            }
        }
        homeCountA= 0;
        homeOffsetA++;
    }
    
    
        bool Bhomed = false;
    int homeCountB = 0;
    int homeOffsetB = 0;
    while(!Bhomed) {
        OD1.SetPosition(0,homeOffsetB);
        Thread::wait(1);
        for(int i = 0 ; i < 99; i++) {
            if(homeSwitchB == false) {
                homeCountB++;
            } else {
                homeCountB = 0;
            }
            if(homeCountB > 100) {
                Bhomed = true;
            }
        }
        homeCountB= 0;
        homeOffsetB++;
    }

    while(true) {
        OD0.SetPosition(0,1200);
        OD1.SetPosition(1,1200);
        OD1.SetPosition(0,1200);
        Thread::wait(1000);
        OD0.SetPosition(0,-2000);
        OD1.SetPosition(1,-2000);
        OD1.SetPosition(0,-2000);
        Thread::wait(1000);
        string busVrequest = "r vbus_voltage\n";
        ODSerial0.write(busVrequest.c_str(),busVrequest.length());
        buffered_pc.printf("bus voltage %f\n",OD0.readFloat());
    }

}


int main()
{
    buffered_pc.baud(115200);
    buffered_pc.printf("hello\r\n");
    Thread transmitter;
    Thread receiver;
    Thread odriveThread;

    //set switches up
    homeGND = false;
    homeSwitchA.mode(PullUp);
    homeSwitchB.mode(PullUp);
    homeSwitchC.mode(PullUp);
    trigger.mode(PullUp);
    //eth.connect();

    //buffered_pc.printf("Controller IP Address is %s\r\n", eth.get_ip_address());
    Thread::wait(5000);

    //transmitter.start(transmit);
    //receiver.start(receive);
    odriveThread.start(runOdrive);

    while (true) {
        led1 = !led1;
        Thread::wait(500);
    }
}