Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
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?
User | Revision | Line number | New 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(ð); |
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(ð); |
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 | } |