Diff: main.cpp

Revision:

3:10fa3102c2d7

Parent:

1:965d7fb768b6

Child:

4:778bc352c47f

--- a/main.cpp	Fri Jun 23 14:13:09 2017 -0500
+++ b/main.cpp	Fri Oct 05 15:57:55 2018 +0000
@@ -1,38 +1,168 @@
 #include "mbed.h"
-#include "EthernetInterface.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);
 
-// Network interface
-EthernetInterface net;
+using std::string;
+const int BROADCAST_PORT_T = 58080;
+const int BROADCAST_PORT_R = 58081;
+EthernetInterface eth;
+
+BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX);
+
 
-// Socket demo
-int main() {
-    // Bring up the ethernet interface
-    printf("Ethernet socket example\n");
-    net.connect();
+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);
+    }
+}
 
-    // Show the network address
-    const char *ip = net.get_ip_address();
-    printf("IP address is: %s\n", ip ? ip : "No IP");
+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)
+{
+
+}
 
-    // Open a socket on the network interface, and create a TCP connection to mbed.org
-    TCPSocket socket;
-    socket.open(&net);
-    socket.connect("www.arm.com", 80);
-
-    // Send a simple http request
-    char sbuffer[] = "GET / HTTP/1.1\r\nHost: www.arm.com\r\n\r\n";
-    int scount = socket.send(sbuffer, sizeof sbuffer);
-    printf("sent %d [%.*s]\n", scount, strstr(sbuffer, "\r\n")-sbuffer, sbuffer);
+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++;
+    }
 
-    // Recieve a simple http response and print out the response line
-    char rbuffer[64];
-    int rcount = socket.recv(rbuffer, sizeof rbuffer);
-    printf("recv %d [%.*s]\n", rcount, strstr(rbuffer, "\r\n")-rbuffer, rbuffer);
+    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());
+    }
+
+}
+
 
-    // Close the socket to return its memory and bring down the network interface
-    socket.close();
+int main()
+{
+    buffered_pc.baud(115200);
+    buffered_pc.printf("hello\r\n");
+    Thread transmitter;
+    Thread receiver;
+    Thread odriveThread;
 
-    // Bring down the ethernet interface
-    net.disconnect();
-    printf("Done\n");
+    //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);
+    }
 }