File content as of revision 4:778bc352c47f:

#include "mbed.h"
#include "odrive.h"
#include "lwip-interface/EthernetInterface.h"
#include "comms.h"
#include <string>
#include "calibration.h"
#include "Axis.h"
#include "kinematics.h"
#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);
BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX);
calVals calibration;


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




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);
    //Axis A(&OD1, 1, &homeSwitchA,calibration,AX_A);
//    Axis B(&OD1, 0, &homeSwitchB,calibration,AX_B);
//    Axis C(&OD0, 0, &homeSwitchC,calibration,AX_C);
//    Kinematics kin(&A, &B, &C, calibration); // the Kinematics class contains everything
//    kin.activateMotors();
//    kin.homeMotors();
//    //int error = kin.goToPos(0,0,-10);
//    kin.goToAngles(pi/4,pi/4,pi/4);
//    Thread::wait(1000);
//    float inc = pi /500;
//    float i = 0;
//    float k = -100;
//    int up = 0;
    while(true) {
 //       int count = 0;
//        for(int j = 0 ; j < 100; j++) {
//
//            if(trigger == false) {
//                count++;
//            }
//        }
//        if(count>90) {
//            i += inc;
//            if(up == 1)k+=0.05;
//            else k-=0.05;
//            if(k >= -80) up = 0;
//            if(k <= -180) up = 1;
//            //int error = kin.goToPos(40.0*sin(i),40.0*cos(i),k);
//            //int error = kin.goToPos(0,0,k);
//            if(i > 2*pi) i = 0;
            Thread::wait(10);
            buffered_pc.printf("bus voltage %f\r\n",OD1.readBattery());
            //buffered_pc.printf("k= %f\r\n",k);
        //}
    }

}


int main()
{

    calibration.e = 58.095;
    calibration.f = 60.722;
    calibration.re = 150;
    calibration.rf = 143.0;
    calibration.Aoffset = 0.43 - 0.111701;
    calibration.Boffset = 0.43 - 0.111701;
    calibration.Coffset = 0.43 - 0.111701;
    calibration.gearRatio = 89.0/24.0;
    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);
    }
}