giovanni beri / Mbed OS ROBI_fw

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   X_NUCLEO_IKS01A1

main.cpp@13:04fa45b2bee1, 2018-05-11 (annotated)

Committer:
giovanniberi93
Date:
Fri May 11 08:04:59 2018 +0000
Revision:
13:04fa45b2bee1
Parent:
12:101a11e0e37e 
No significant changes, commit only to publish the project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
giovanniberi93 0:61f2110838dc 1 #include "mbed.h"
giovanniberi93 3:8a744d554a2f 2 #include "x_nucleo_iks01a1.h"
giovanniberi93 0:61f2110838dc 3 #include "EthernetInterface.h"
giovanniberi93 0:61f2110838dc 4 #include "UDPSocket.h"
giovanniberi93 2:2d6a1a09c59f 5 #include "param_config.h"
giovanniberi93 9:7326e6e9c09c 6 #include "CAN.h"
giovanniberi93 5:930185bdb68f 7
giovanniberi93 9:7326e6e9c09c 8 // Instantiate the expansion board
giovanniberi93 3:8a744d554a2f 9 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
giovanniberi93 9:7326e6e9c09c 10 // Retrieve the composing elements of the expansion board
giovanniberi93 3:8a744d554a2f 11 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
giovanniberi93 3:8a744d554a2f 12 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
giovanniberi93 3:8a744d554a2f 13 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
giovanniberi93 0:61f2110838dc 14
giovanniberi93 9:7326e6e9c09c 15 // other peripherals
giovanniberi93 10:de739d701a32 16 CAN enginesCAN(PD_0, PD_1, 1000000);
giovanniberi93 5:930185bdb68f 17 EthernetInterface eth;
giovanniberi93 9:7326e6e9c09c 18 UDPSocket receive_sock;
giovanniberi93 9:7326e6e9c09c 19 UDPSocket send_sock;
giovanniberi93 0:61f2110838dc 20 Serial pc(USBTX, USBRX);
giovanniberi93 0:61f2110838dc 21
giovanniberi93 12:101a11e0e37e 22 // CANMessages
giovanniberi93 12:101a11e0e37e 23 CANMessage frontRightWheel;
giovanniberi93 12:101a11e0e37e 24 CANMessage frontLeftWheel;
giovanniberi93 12:101a11e0e37e 25 CANMessage rearRightWheel;
giovanniberi93 12:101a11e0e37e 26 CANMessage rearLeftWheel;
giovanniberi93 12:101a11e0e37e 27 CANMessage receivedCANMsg;
giovanniberi93 12:101a11e0e37e 28
giovanniberi93 9:7326e6e9c09c 29 //----------------------------------------------------------
giovanniberi93 9:7326e6e9c09c 30 //----------------------------------------------------------
giovanniberi93 9:7326e6e9c09c 31
giovanniberi93 5:930185bdb68f 32 void setupExpansionBoard(){
giovanniberi93 3:8a744d554a2f 33 uint8_t id;
giovanniberi93 3:8a744d554a2f 34 magnetometer->read_id(&id);
giovanniberi93 3:8a744d554a2f 35 if(!id)
giovanniberi93 3:8a744d554a2f 36 pc.printf("Magnetometer not detected");
giovanniberi93 3:8a744d554a2f 37 gyroscope->read_id(&id);
giovanniberi93 3:8a744d554a2f 38 if(!id)
giovanniberi93 3:8a744d554a2f 39 pc.printf("Gyroscope not detected");
giovanniberi93 3:8a744d554a2f 40 accelerometer->read_id(&id);
giovanniberi93 3:8a744d554a2f 41 if(!id)
giovanniberi93 3:8a744d554a2f 42 pc.printf("Accelerometer not detected");
giovanniberi93 3:8a744d554a2f 43 }
giovanniberi93 3:8a744d554a2f 44
giovanniberi93 9:7326e6e9c09c 45 void setupEthernet(){
giovanniberi93 9:7326e6e9c09c 46 eth.set_network(DEVICE_IP, "255.255.255.0", "192.168.1.11");
giovanniberi93 9:7326e6e9c09c 47 eth.connect();
giovanniberi93 9:7326e6e9c09c 48 /* Open the server on ethernet stack */
giovanniberi93 9:7326e6e9c09c 49 receive_sock.open(&eth);
giovanniberi93 9:7326e6e9c09c 50 send_sock.open(&eth);
giovanniberi93 9:7326e6e9c09c 51 /* set receiving socket params */
giovanniberi93 9:7326e6e9c09c 52 receive_sock.bind(DEVICE_IP, RECEIVE_PORT);
giovanniberi93 9:7326e6e9c09c 53 receive_sock.set_blocking(false);
giovanniberi93 9:7326e6e9c09c 54 }
giovanniberi93 9:7326e6e9c09c 55
giovanniberi93 12:101a11e0e37e 56 void prepareCANMessages(){
giovanniberi93 12:101a11e0e37e 57 frontRightWheel.format = CANExtended;
giovanniberi93 12:101a11e0e37e 58 frontLeftWheel.format = CANExtended;
giovanniberi93 12:101a11e0e37e 59 rearRightWheel.format = CANExtended;
giovanniberi93 12:101a11e0e37e 60 rearLeftWheel.format = CANExtended;
giovanniberi93 12:101a11e0e37e 61 // data size is the same (4) for all messages for writing speeds, different for reading speed (9)
giovanniberi93 12:101a11e0e37e 62 frontRightWheel.len = 4;
giovanniberi93 12:101a11e0e37e 63 frontLeftWheel.len = 4;
giovanniberi93 12:101a11e0e37e 64 rearRightWheel.len = 4;
giovanniberi93 12:101a11e0e37e 65 rearLeftWheel.len = 4;
giovanniberi93 12:101a11e0e37e 66 // in ID fields, left 8 bits are for the command, right 8 bits for device id
giovanniberi93 12:101a11e0e37e 67 frontRightWheel.id = 0x03 << 8 | 0x02;
giovanniberi93 12:101a11e0e37e 68 frontLeftWheel.id = 0x03 << 8 | 0x01;
giovanniberi93 12:101a11e0e37e 69 rearRightWheel.id = 0x03 << 8 | 0x04;
giovanniberi93 12:101a11e0e37e 70 rearLeftWheel.id = 0x03 << 8 | 0x03;
giovanniberi93 12:101a11e0e37e 71 }
giovanniberi93 9:7326e6e9c09c 72
giovanniberi93 12:101a11e0e37e 73 void writeCANMessages(int32_t leftVel, int32_t rightVel){
giovanniberi93 12:101a11e0e37e 74 // assign data field
giovanniberi93 12:101a11e0e37e 75 frontRightWheel.data[0] = rightVel;
giovanniberi93 12:101a11e0e37e 76 rearRightWheel.data[0] = rightVel;
giovanniberi93 12:101a11e0e37e 77 frontLeftWheel.data[0] = leftVel;
giovanniberi93 12:101a11e0e37e 78 rearLeftWheel.data[0] = leftVel;
giovanniberi93 12:101a11e0e37e 79 // send the messages
giovanniberi93 12:101a11e0e37e 80 enginesCAN.write(frontRightWheel);
giovanniberi93 12:101a11e0e37e 81 enginesCAN.write(rearRightWheel);
giovanniberi93 12:101a11e0e37e 82 enginesCAN.write(frontLeftWheel);
giovanniberi93 12:101a11e0e37e 83 enginesCAN.write(rearLeftWheel);
giovanniberi93 12:101a11e0e37e 84 }
giovanniberi93 9:7326e6e9c09c 85 //----------------------------------------------------------
giovanniberi93 9:7326e6e9c09c 86 //----------------------------------------------------------
giovanniberi93 9:7326e6e9c09c 87
giovanniberi93 9:7326e6e9c09c 88
giovanniberi93 0:61f2110838dc 89 int main()
giovanniberi93 0:61f2110838dc 90 {
giovanniberi93 10:de739d701a32 91 // ethernet
giovanniberi93 9:7326e6e9c09c 92 SocketAddress dest_addr(PC_IP, DESTINATION_PORT);
giovanniberi93 9:7326e6e9c09c 93 SocketAddress tmp_addr;
giovanniberi93 3:8a744d554a2f 94 char send_buf[UDP_SEND_PACKET_SIZE];
giovanniberi93 3:8a744d554a2f 95 char receive_buf[UDP_RECEIVE_PACKET_SIZE];
giovanniberi93 10:de739d701a32 96 // IMU
giovanniberi93 3:8a744d554a2f 97 int32_t axes_mag[3];
giovanniberi93 3:8a744d554a2f 98 int32_t axes_gyro[3];
giovanniberi93 3:8a744d554a2f 99 int32_t axes_acc[3];
giovanniberi93 11:435e9b3e98f9 100 // received velocities
giovanniberi93 11:435e9b3e98f9 101 int32_t* velocities;
giovanniberi93 3:8a744d554a2f 102
giovanniberi93 5:930185bdb68f 103 pc.baud(SERIAL_BAUD);
giovanniberi93 3:8a744d554a2f 104 pc.printf("\n\r***** NEW RUN *****\n\r");
giovanniberi93 0:61f2110838dc 105
giovanniberi93 5:930185bdb68f 106 setupExpansionBoard();
giovanniberi93 9:7326e6e9c09c 107 setupEthernet();
giovanniberi93 12:101a11e0e37e 108 prepareCANMessages();
giovanniberi93 3:8a744d554a2f 109 pc.printf("The board IP address is '%s'\n\r", eth.get_ip_address());
giovanniberi93 3:8a744d554a2f 110 pc.printf("The destination PC address is '%s'\n\r", PC_IP);
giovanniberi93 9:7326e6e9c09c 111
giovanniberi93 3:8a744d554a2f 112 // main loop
giovanniberi93 7:76bc8bf432a1 113 pc.printf("Streaming data to address %s:%d \n\r", PC_IP, DESTINATION_PORT);
giovanniberi93 10:de739d701a32 114 pc.printf("Structure of data string:\n\r timestamp axes_mag[0] axes_mag[1] axes_mag[2] axes_acc[0] axes_acc[1] axes_acc[2] axes_gyro[0] axes_gyro[1] axes_gyro[2]\n\r");
giovanniberi93 0:61f2110838dc 115 while (true) {
giovanniberi93 3:8a744d554a2f 116 // tmp_addr get filled with the address of the sender of received message
giovanniberi93 3:8a744d554a2f 117 if(receive_sock.recvfrom(&tmp_addr, receive_buf, UDP_RECEIVE_PACKET_SIZE) > 0) {
giovanniberi93 2:2d6a1a09c59f 118 // handle received data
giovanniberi93 11:435e9b3e98f9 119 velocities = reinterpret_cast<int32_t*>(receive_buf);
giovanniberi93 10:de739d701a32 120 pc.printf("Ricevuto da ethernet\n\r");
giovanniberi93 11:435e9b3e98f9 121 pc.printf("%d %d \n\r", velocities[0], velocities[1]);
giovanniberi93 12:101a11e0e37e 122 writeCANMessages(velocities[0], velocities[1]);
giovanniberi93 10:de739d701a32 123 pc.printf("Scritto su CAN \n\r");
giovanniberi93 12:101a11e0e37e 124 if(enginesCAN.read(receivedCANMsg)){
giovanniberi93 10:de739d701a32 125 pc.printf("Can loopback va \n\r");
giovanniberi93 10:de739d701a32 126 } else {
giovanniberi93 10:de739d701a32 127 pc.printf("Can loopback NON va \n\r");
giovanniberi93 10:de739d701a32 128 }
giovanniberi93 10:de739d701a32 129
giovanniberi93 2:2d6a1a09c59f 130 }
giovanniberi93 3:8a744d554a2f 131 // read from sensors
giovanniberi93 4:417f5986b087 132 magnetometer-> get_m_axes(axes_mag);
giovanniberi93 4:417f5986b087 133 accelerometer-> get_x_axes(axes_acc);
giovanniberi93 4:417f5986b087 134 gyroscope-> get_g_axes(axes_gyro);
giovanniberi93 7:76bc8bf432a1 135 sprintf(send_buf, "%d %d %d %d %d %d %d %d %d ", axes_mag[0], axes_mag[1], axes_mag[2], axes_acc[0], axes_acc[1], axes_acc[2], axes_gyro[0], axes_gyro[1], axes_gyro[2]);
giovanniberi93 4:417f5986b087 136 send_sock.sendto(dest_addr, (void *)send_buf, 350);
giovanniberi93 0:61f2110838dc 137
giovanniberi93 4:417f5986b087 138 // Spinning at highest speed. Currently the rate is about 166Hz
giovanniberi93 9:7326e6e9c09c 139 // wait(0.1);
giovanniberi93 0:61f2110838dc 140 }
giovanniberi93 0:61f2110838dc 141 }