joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

main.cpp@3:10fa3102c2d7, 2018-10-05 (annotated)

Committer:
je310
Date:
Fri Oct 05 15:57:55 2018 +0000
Revision:
3:10fa3102c2d7
Parent:
1:965d7fb768b6
Child:
4:778bc352c47f
Delta firmware using odrives.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mab5449 0:6b383744246e 1 #include "mbed.h"
je310 3:10fa3102c2d7 2 #include "odrive.h"
je310 3:10fa3102c2d7 3 #include "lwip-interface/EthernetInterface.h"
je310 3:10fa3102c2d7 4 #include "comms.h"
je310 3:10fa3102c2d7 5 #include <string>
je310 3:10fa3102c2d7 6 #include "BufferedSerial.h"
je310 3:10fa3102c2d7 7 DigitalOut led1(LED1);
je310 3:10fa3102c2d7 8 DigitalOut homeGND(PF_5);
je310 3:10fa3102c2d7 9 DigitalIn homeSwitchA(PA_3);
je310 3:10fa3102c2d7 10 DigitalIn homeSwitchB(PC_0);
je310 3:10fa3102c2d7 11 DigitalIn homeSwitchC(PC_3);
je310 3:10fa3102c2d7 12 DigitalIn trigger(PF_3);
mab5449 0:6b383744246e 13
je310 3:10fa3102c2d7 14 using std::string;
je310 3:10fa3102c2d7 15 const int BROADCAST_PORT_T = 58080;
je310 3:10fa3102c2d7 16 const int BROADCAST_PORT_R = 58081;
je310 3:10fa3102c2d7 17 EthernetInterface eth;
je310 3:10fa3102c2d7 18
je310 3:10fa3102c2d7 19 BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX);
je310 3:10fa3102c2d7 20
mab5449 0:6b383744246e 21
je310 3:10fa3102c2d7 22 void transmit()
je310 3:10fa3102c2d7 23 {
je310 3:10fa3102c2d7 24 UDPSocket socket(&eth);
je310 3:10fa3102c2d7 25 string out_buffer = "very important data";
je310 3:10fa3102c2d7 26 SocketAddress transmit("10.0.0.160", BROADCAST_PORT_T);
je310 3:10fa3102c2d7 27 fromRobot msg;
je310 3:10fa3102c2d7 28 msg.motor1.busVoltage = 69;
je310 3:10fa3102c2d7 29 while (true) {
je310 3:10fa3102c2d7 30 msg.motor1.busVoltage += 1;
je310 3:10fa3102c2d7 31 int ret = socket.sendto(transmit, &msg, sizeof(msg));
je310 3:10fa3102c2d7 32 //printf("sendto return: %d\n", ret);
je310 3:10fa3102c2d7 33
je310 3:10fa3102c2d7 34 Thread::wait(100);
je310 3:10fa3102c2d7 35 }
je310 3:10fa3102c2d7 36 }
mab5449 0:6b383744246e 37
je310 3:10fa3102c2d7 38 void receive()
je310 3:10fa3102c2d7 39 {
je310 3:10fa3102c2d7 40 SocketAddress receive;
je310 3:10fa3102c2d7 41 UDPSocket socket(&eth);
je310 3:10fa3102c2d7 42 int bind = socket.bind(BROADCAST_PORT_R);
je310 3:10fa3102c2d7 43 //printf("bind return: %d", bind);
je310 3:10fa3102c2d7 44
je310 3:10fa3102c2d7 45 char buffer[256];
je310 3:10fa3102c2d7 46 while (true) {
je310 3:10fa3102c2d7 47 //printf("\nWait for packet...\n");
je310 3:10fa3102c2d7 48 int n = socket.recvfrom(&receive, buffer, sizeof(buffer));
je310 3:10fa3102c2d7 49 toRobot inmsg;
je310 3:10fa3102c2d7 50 memcpy(&inmsg, buffer, n);
je310 3:10fa3102c2d7 51 //buffer[n] = '\0';
je310 3:10fa3102c2d7 52 buffered_pc.printf("Count:%i, Motor1Vel:%f, Motor1Tor: %f \r\n",inmsg.count, inmsg.motor1.velocity,inmsg.motor1.torque);
je310 3:10fa3102c2d7 53
je310 3:10fa3102c2d7 54 //Thread::wait(1);
je310 3:10fa3102c2d7 55 }
je310 3:10fa3102c2d7 56 }
je310 3:10fa3102c2d7 57
je310 3:10fa3102c2d7 58 int getCountPos(BufferedSerial ser, int axis)
je310 3:10fa3102c2d7 59 {
je310 3:10fa3102c2d7 60
je310 3:10fa3102c2d7 61 }
mab5449 0:6b383744246e 62
je310 3:10fa3102c2d7 63 void runOdrive()
je310 3:10fa3102c2d7 64 {
je310 3:10fa3102c2d7 65 BufferedSerial ODSerial0(PC_12,PD_2);
je310 3:10fa3102c2d7 66 ODSerial0.baud(115200);
je310 3:10fa3102c2d7 67 BufferedSerial ODSerial1(PD_5,PD_6);
je310 3:10fa3102c2d7 68 ODSerial1.baud(115200);
je310 3:10fa3102c2d7 69 ODrive OD0(ODSerial0);
je310 3:10fa3102c2d7 70 ODrive OD1(ODSerial1);
je310 3:10fa3102c2d7 71 int requested_state;
je310 3:10fa3102c2d7 72 requested_state = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL;
je310 3:10fa3102c2d7 73 OD0.run_state(0, requested_state, false); // don't wait
je310 3:10fa3102c2d7 74 OD1.run_state(0, requested_state, false); // don't wait
je310 3:10fa3102c2d7 75 OD1.run_state(1, requested_state, false); // don't wait
je310 3:10fa3102c2d7 76 Thread::wait(1000);
je310 3:10fa3102c2d7 77 // OD0.SetVelocity(0,-1000);
je310 3:10fa3102c2d7 78 // OD1.SetVelocity(1,1000);
je310 3:10fa3102c2d7 79 //OD1.SetVelocity(1,1000);
je310 3:10fa3102c2d7 80 // OD1.SetVelocity(0,1000);
je310 3:10fa3102c2d7 81 // OD1.SetVelocity(1,1000);
je310 3:10fa3102c2d7 82
je310 3:10fa3102c2d7 83 bool Ahomed = false;
je310 3:10fa3102c2d7 84 int homeCountA = 0;
je310 3:10fa3102c2d7 85 int homeOffsetA = 0;
je310 3:10fa3102c2d7 86 while(!Ahomed) {
je310 3:10fa3102c2d7 87 OD1.SetPosition(1,homeOffsetA);
je310 3:10fa3102c2d7 88 Thread::wait(1);
je310 3:10fa3102c2d7 89 for(int i = 0 ; i < 99; i++) {
je310 3:10fa3102c2d7 90 if(homeSwitchA == false) {
je310 3:10fa3102c2d7 91 homeCountA++;
je310 3:10fa3102c2d7 92 } else {
je310 3:10fa3102c2d7 93 homeCountA = 0;
je310 3:10fa3102c2d7 94 }
je310 3:10fa3102c2d7 95 if(homeCountA > 100) {
je310 3:10fa3102c2d7 96 Ahomed = true;
je310 3:10fa3102c2d7 97 }
je310 3:10fa3102c2d7 98 }
je310 3:10fa3102c2d7 99 homeCountA= 0;
je310 3:10fa3102c2d7 100 homeOffsetA++;
je310 3:10fa3102c2d7 101 }
je310 3:10fa3102c2d7 102
je310 3:10fa3102c2d7 103
je310 3:10fa3102c2d7 104 bool Bhomed = false;
je310 3:10fa3102c2d7 105 int homeCountB = 0;
je310 3:10fa3102c2d7 106 int homeOffsetB = 0;
je310 3:10fa3102c2d7 107 while(!Bhomed) {
je310 3:10fa3102c2d7 108 OD1.SetPosition(0,homeOffsetB);
je310 3:10fa3102c2d7 109 Thread::wait(1);
je310 3:10fa3102c2d7 110 for(int i = 0 ; i < 99; i++) {
je310 3:10fa3102c2d7 111 if(homeSwitchB == false) {
je310 3:10fa3102c2d7 112 homeCountB++;
je310 3:10fa3102c2d7 113 } else {
je310 3:10fa3102c2d7 114 homeCountB = 0;
je310 3:10fa3102c2d7 115 }
je310 3:10fa3102c2d7 116 if(homeCountB > 100) {
je310 3:10fa3102c2d7 117 Bhomed = true;
je310 3:10fa3102c2d7 118 }
je310 3:10fa3102c2d7 119 }
je310 3:10fa3102c2d7 120 homeCountB= 0;
je310 3:10fa3102c2d7 121 homeOffsetB++;
je310 3:10fa3102c2d7 122 }
mab5449 0:6b383744246e 123
je310 3:10fa3102c2d7 124 while(true) {
je310 3:10fa3102c2d7 125 OD0.SetPosition(0,1200);
je310 3:10fa3102c2d7 126 OD1.SetPosition(1,1200);
je310 3:10fa3102c2d7 127 OD1.SetPosition(0,1200);
je310 3:10fa3102c2d7 128 Thread::wait(1000);
je310 3:10fa3102c2d7 129 OD0.SetPosition(0,-2000);
je310 3:10fa3102c2d7 130 OD1.SetPosition(1,-2000);
je310 3:10fa3102c2d7 131 OD1.SetPosition(0,-2000);
je310 3:10fa3102c2d7 132 Thread::wait(1000);
je310 3:10fa3102c2d7 133 string busVrequest = "r vbus_voltage\n";
je310 3:10fa3102c2d7 134 ODSerial0.write(busVrequest.c_str(),busVrequest.length());
je310 3:10fa3102c2d7 135 buffered_pc.printf("bus voltage %f\n",OD0.readFloat());
je310 3:10fa3102c2d7 136 }
je310 3:10fa3102c2d7 137
je310 3:10fa3102c2d7 138 }
je310 3:10fa3102c2d7 139
mab5449 0:6b383744246e 140
je310 3:10fa3102c2d7 141 int main()
je310 3:10fa3102c2d7 142 {
je310 3:10fa3102c2d7 143 buffered_pc.baud(115200);
je310 3:10fa3102c2d7 144 buffered_pc.printf("hello\r\n");
je310 3:10fa3102c2d7 145 Thread transmitter;
je310 3:10fa3102c2d7 146 Thread receiver;
je310 3:10fa3102c2d7 147 Thread odriveThread;
mab5449 0:6b383744246e 148
je310 3:10fa3102c2d7 149 //set switches up
je310 3:10fa3102c2d7 150 homeGND = false;
je310 3:10fa3102c2d7 151 homeSwitchA.mode(PullUp);
je310 3:10fa3102c2d7 152 homeSwitchB.mode(PullUp);
je310 3:10fa3102c2d7 153 homeSwitchC.mode(PullUp);
je310 3:10fa3102c2d7 154 trigger.mode(PullUp);
je310 3:10fa3102c2d7 155 //eth.connect();
je310 3:10fa3102c2d7 156
je310 3:10fa3102c2d7 157 //buffered_pc.printf("Controller IP Address is %s\r\n", eth.get_ip_address());
je310 3:10fa3102c2d7 158 Thread::wait(5000);
je310 3:10fa3102c2d7 159
je310 3:10fa3102c2d7 160 //transmitter.start(transmit);
je310 3:10fa3102c2d7 161 //receiver.start(receive);
je310 3:10fa3102c2d7 162 odriveThread.start(runOdrive);
je310 3:10fa3102c2d7 163
je310 3:10fa3102c2d7 164 while (true) {
je310 3:10fa3102c2d7 165 led1 = !led1;
je310 3:10fa3102c2d7 166 Thread::wait(500);
je310 3:10fa3102c2d7 167 }
mab5449 0:6b383744246e 168 }