joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

main.cpp@4:778bc352c47f, 2018-10-07 (annotated)

Committer:
je310
Date:
Sun Oct 07 19:40:12 2018 +0000
Revision:
4:778bc352c47f
Parent:
3:10fa3102c2d7
Child:
5:01e1e68309ae
can work smoothly, though voltage reading not working;

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 4:778bc352c47f 6 #include "calibration.h"
je310 4:778bc352c47f 7 #include "Axis.h"
je310 4:778bc352c47f 8 #include "kinematics.h"
je310 3:10fa3102c2d7 9 #include "BufferedSerial.h"
je310 3:10fa3102c2d7 10 DigitalOut led1(LED1);
je310 3:10fa3102c2d7 11 DigitalOut homeGND(PF_5);
je310 3:10fa3102c2d7 12 DigitalIn homeSwitchA(PA_3);
je310 3:10fa3102c2d7 13 DigitalIn homeSwitchB(PC_0);
je310 3:10fa3102c2d7 14 DigitalIn homeSwitchC(PC_3);
je310 3:10fa3102c2d7 15 DigitalIn trigger(PF_3);
je310 4:778bc352c47f 16 BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX);
je310 4:778bc352c47f 17 calVals calibration;
je310 4:778bc352c47f 18
mab5449 0:6b383744246e 19
je310 3:10fa3102c2d7 20 using std::string;
je310 3:10fa3102c2d7 21 const int BROADCAST_PORT_T = 58080;
je310 3:10fa3102c2d7 22 const int BROADCAST_PORT_R = 58081;
je310 3:10fa3102c2d7 23 EthernetInterface eth;
je310 3:10fa3102c2d7 24
je310 4:778bc352c47f 25
je310 3:10fa3102c2d7 26
mab5449 0:6b383744246e 27
je310 3:10fa3102c2d7 28 void transmit()
je310 3:10fa3102c2d7 29 {
je310 3:10fa3102c2d7 30 UDPSocket socket(&eth);
je310 3:10fa3102c2d7 31 string out_buffer = "very important data";
je310 3:10fa3102c2d7 32 SocketAddress transmit("10.0.0.160", BROADCAST_PORT_T);
je310 3:10fa3102c2d7 33 fromRobot msg;
je310 3:10fa3102c2d7 34 msg.motor1.busVoltage = 69;
je310 3:10fa3102c2d7 35 while (true) {
je310 3:10fa3102c2d7 36 msg.motor1.busVoltage += 1;
je310 3:10fa3102c2d7 37 int ret = socket.sendto(transmit, &msg, sizeof(msg));
je310 3:10fa3102c2d7 38 //printf("sendto return: %d\n", ret);
je310 3:10fa3102c2d7 39
je310 3:10fa3102c2d7 40 Thread::wait(100);
je310 3:10fa3102c2d7 41 }
je310 3:10fa3102c2d7 42 }
mab5449 0:6b383744246e 43
je310 3:10fa3102c2d7 44 void receive()
je310 3:10fa3102c2d7 45 {
je310 3:10fa3102c2d7 46 SocketAddress receive;
je310 3:10fa3102c2d7 47 UDPSocket socket(&eth);
je310 3:10fa3102c2d7 48 int bind = socket.bind(BROADCAST_PORT_R);
je310 3:10fa3102c2d7 49 //printf("bind return: %d", bind);
je310 3:10fa3102c2d7 50
je310 3:10fa3102c2d7 51 char buffer[256];
je310 3:10fa3102c2d7 52 while (true) {
je310 3:10fa3102c2d7 53 //printf("\nWait for packet...\n");
je310 3:10fa3102c2d7 54 int n = socket.recvfrom(&receive, buffer, sizeof(buffer));
je310 3:10fa3102c2d7 55 toRobot inmsg;
je310 3:10fa3102c2d7 56 memcpy(&inmsg, buffer, n);
je310 3:10fa3102c2d7 57 //buffer[n] = '\0';
je310 3:10fa3102c2d7 58 buffered_pc.printf("Count:%i, Motor1Vel:%f, Motor1Tor: %f \r\n",inmsg.count, inmsg.motor1.velocity,inmsg.motor1.torque);
je310 3:10fa3102c2d7 59
je310 3:10fa3102c2d7 60 //Thread::wait(1);
je310 3:10fa3102c2d7 61 }
je310 3:10fa3102c2d7 62 }
je310 3:10fa3102c2d7 63
je310 3:10fa3102c2d7 64 int getCountPos(BufferedSerial ser, int axis)
je310 3:10fa3102c2d7 65 {
je310 3:10fa3102c2d7 66
je310 3:10fa3102c2d7 67 }
mab5449 0:6b383744246e 68
je310 3:10fa3102c2d7 69 void runOdrive()
je310 3:10fa3102c2d7 70 {
je310 3:10fa3102c2d7 71 BufferedSerial ODSerial0(PC_12,PD_2);
je310 3:10fa3102c2d7 72 ODSerial0.baud(115200);
je310 3:10fa3102c2d7 73 BufferedSerial ODSerial1(PD_5,PD_6);
je310 3:10fa3102c2d7 74 ODSerial1.baud(115200);
je310 3:10fa3102c2d7 75 ODrive OD0(ODSerial0);
je310 3:10fa3102c2d7 76 ODrive OD1(ODSerial1);
je310 4:778bc352c47f 77 //Axis A(&OD1, 1, &homeSwitchA,calibration,AX_A);
je310 4:778bc352c47f 78 // Axis B(&OD1, 0, &homeSwitchB,calibration,AX_B);
je310 4:778bc352c47f 79 // Axis C(&OD0, 0, &homeSwitchC,calibration,AX_C);
je310 4:778bc352c47f 80 // Kinematics kin(&A, &B, &C, calibration); // the Kinematics class contains everything
je310 4:778bc352c47f 81 // kin.activateMotors();
je310 4:778bc352c47f 82 // kin.homeMotors();
je310 4:778bc352c47f 83 // //int error = kin.goToPos(0,0,-10);
je310 4:778bc352c47f 84 // kin.goToAngles(pi/4,pi/4,pi/4);
je310 4:778bc352c47f 85 // Thread::wait(1000);
je310 4:778bc352c47f 86 // float inc = pi /500;
je310 4:778bc352c47f 87 // float i = 0;
je310 4:778bc352c47f 88 // float k = -100;
je310 4:778bc352c47f 89 // int up = 0;
je310 3:10fa3102c2d7 90 while(true) {
je310 4:778bc352c47f 91 // int count = 0;
je310 4:778bc352c47f 92 // for(int j = 0 ; j < 100; j++) {
je310 4:778bc352c47f 93 //
je310 4:778bc352c47f 94 // if(trigger == false) {
je310 4:778bc352c47f 95 // count++;
je310 4:778bc352c47f 96 // }
je310 4:778bc352c47f 97 // }
je310 4:778bc352c47f 98 // if(count>90) {
je310 4:778bc352c47f 99 // i += inc;
je310 4:778bc352c47f 100 // if(up == 1)k+=0.05;
je310 4:778bc352c47f 101 // else k-=0.05;
je310 4:778bc352c47f 102 // if(k >= -80) up = 0;
je310 4:778bc352c47f 103 // if(k <= -180) up = 1;
je310 4:778bc352c47f 104 // //int error = kin.goToPos(40.0*sin(i),40.0*cos(i),k);
je310 4:778bc352c47f 105 // //int error = kin.goToPos(0,0,k);
je310 4:778bc352c47f 106 // if(i > 2*pi) i = 0;
je310 4:778bc352c47f 107 Thread::wait(10);
je310 4:778bc352c47f 108 buffered_pc.printf("bus voltage %f\r\n",OD1.readBattery());
je310 4:778bc352c47f 109 //buffered_pc.printf("k= %f\r\n",k);
je310 4:778bc352c47f 110 //}
je310 3:10fa3102c2d7 111 }
je310 3:10fa3102c2d7 112
je310 3:10fa3102c2d7 113 }
je310 3:10fa3102c2d7 114
mab5449 0:6b383744246e 115
je310 3:10fa3102c2d7 116 int main()
je310 3:10fa3102c2d7 117 {
je310 4:778bc352c47f 118
je310 4:778bc352c47f 119 calibration.e = 58.095;
je310 4:778bc352c47f 120 calibration.f = 60.722;
je310 4:778bc352c47f 121 calibration.re = 150;
je310 4:778bc352c47f 122 calibration.rf = 143.0;
je310 4:778bc352c47f 123 calibration.Aoffset = 0.43 - 0.111701;
je310 4:778bc352c47f 124 calibration.Boffset = 0.43 - 0.111701;
je310 4:778bc352c47f 125 calibration.Coffset = 0.43 - 0.111701;
je310 4:778bc352c47f 126 calibration.gearRatio = 89.0/24.0;
je310 3:10fa3102c2d7 127 buffered_pc.baud(115200);
je310 3:10fa3102c2d7 128 buffered_pc.printf("hello\r\n");
je310 3:10fa3102c2d7 129 Thread transmitter;
je310 3:10fa3102c2d7 130 Thread receiver;
je310 3:10fa3102c2d7 131 Thread odriveThread;
mab5449 0:6b383744246e 132
je310 4:778bc352c47f 133
je310 3:10fa3102c2d7 134 //set switches up
je310 3:10fa3102c2d7 135 homeGND = false;
je310 3:10fa3102c2d7 136 homeSwitchA.mode(PullUp);
je310 3:10fa3102c2d7 137 homeSwitchB.mode(PullUp);
je310 3:10fa3102c2d7 138 homeSwitchC.mode(PullUp);
je310 3:10fa3102c2d7 139 trigger.mode(PullUp);
je310 3:10fa3102c2d7 140 //eth.connect();
je310 3:10fa3102c2d7 141
je310 3:10fa3102c2d7 142 //buffered_pc.printf("Controller IP Address is %s\r\n", eth.get_ip_address());
je310 3:10fa3102c2d7 143 Thread::wait(5000);
je310 3:10fa3102c2d7 144
je310 3:10fa3102c2d7 145 //transmitter.start(transmit);
je310 3:10fa3102c2d7 146 //receiver.start(receive);
je310 3:10fa3102c2d7 147 odriveThread.start(runOdrive);
je310 3:10fa3102c2d7 148
je310 3:10fa3102c2d7 149 while (true) {
je310 4:778bc352c47f 150 //led1 = !led1;
je310 3:10fa3102c2d7 151 Thread::wait(500);
je310 3:10fa3102c2d7 152 }
mab5449 0:6b383744246e 153 }