main.cpp

00001 #include "mbed.h"
00002 #include "x_nucleo_iks01a1.h"
00003 #include "EthernetInterface.h"
00004 #include "UDPSocket.h"
00005 #include "param_config.h"
00006 #include "CAN.h"
00007 
00008 // Instantiate the expansion board
00009 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
00010 // Retrieve the composing elements of the expansion board
00011 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
00012 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
00013 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
00014 
00015 // other peripherals
00016 CAN enginesCAN(PD_0, PD_1, 1000000);
00017 EthernetInterface eth;
00018 UDPSocket receive_sock;
00019 UDPSocket send_sock;
00020 Serial pc(USBTX, USBRX);
00021 
00022 // CANMessages
00023 CANMessage frontRightWheel;
00024 CANMessage frontLeftWheel;
00025 CANMessage rearRightWheel;
00026 CANMessage rearLeftWheel;
00027 CANMessage receivedCANMsg;
00028 
00029 //----------------------------------------------------------
00030 //----------------------------------------------------------
00031 
00032 void setupExpansionBoard(){
00033     uint8_t id;
00034     magnetometer->read_id(&id);
00035     if(!id)
00036         pc.printf("Magnetometer not detected");
00037     gyroscope->read_id(&id); 
00038     if(!id)
00039         pc.printf("Gyroscope not detected");
00040     accelerometer->read_id(&id);
00041     if(!id)
00042         pc.printf("Accelerometer not detected");
00043 }
00044 
00045 void setupEthernet(){
00046     eth.set_network(DEVICE_IP, "255.255.255.0", "192.168.1.11");
00047     eth.connect();
00048     /* Open the server on ethernet stack */
00049     receive_sock.open(&eth);
00050     send_sock.open(&eth);    
00051     /* set receiving socket params */
00052     receive_sock.bind(DEVICE_IP, RECEIVE_PORT);
00053     receive_sock.set_blocking(false);    
00054 }
00055 
00056 void prepareCANMessages(){
00057     frontRightWheel.format = CANExtended;
00058     frontLeftWheel.format  = CANExtended;
00059     rearRightWheel.format  = CANExtended;
00060     rearLeftWheel.format   = CANExtended;
00061     // data size is the same (4) for all messages for writing speeds, different for reading speed (9)
00062     frontRightWheel.len = 4;
00063     frontLeftWheel.len  = 4;
00064     rearRightWheel.len  = 4;
00065     rearLeftWheel.len   = 4;
00066     // in ID fields, left 8 bits are for the command, right 8 bits for device id
00067     frontRightWheel.id  = 0x03 << 8 | 0x02;
00068     frontLeftWheel.id   = 0x03 << 8 | 0x01;
00069     rearRightWheel.id   = 0x03 << 8 | 0x04;
00070     rearLeftWheel.id    = 0x03 << 8 | 0x03;            
00071 }
00072 
00073 void writeCANMessages(int32_t leftVel, int32_t rightVel){
00074     // assign data field
00075     frontRightWheel.data[0] = rightVel;
00076     rearRightWheel.data[0]  = rightVel;
00077     frontLeftWheel.data[0]  = leftVel;
00078     rearLeftWheel.data[0]   = leftVel;
00079     // send the messages
00080     enginesCAN.write(frontRightWheel);
00081     enginesCAN.write(rearRightWheel);
00082     enginesCAN.write(frontLeftWheel);
00083     enginesCAN.write(rearLeftWheel);
00084 }
00085 //----------------------------------------------------------
00086 //----------------------------------------------------------
00087 
00088 
00089 int main()
00090 {
00091     // ethernet
00092     SocketAddress dest_addr(PC_IP, DESTINATION_PORT);
00093     SocketAddress tmp_addr;
00094     char send_buf[UDP_SEND_PACKET_SIZE];
00095     char receive_buf[UDP_RECEIVE_PACKET_SIZE];
00096     // IMU
00097     int32_t axes_mag[3];
00098     int32_t axes_gyro[3];
00099     int32_t axes_acc[3];
00100     // received velocities
00101     int32_t* velocities;
00102     
00103     pc.baud(SERIAL_BAUD);
00104     pc.printf("\n\r***** NEW RUN *****\n\r");
00105     
00106     setupExpansionBoard();
00107     setupEthernet();
00108     prepareCANMessages();
00109     pc.printf("The board IP address is '%s'\n\r", eth.get_ip_address());
00110     pc.printf("The destination PC address is '%s'\n\r", PC_IP);
00111 
00112     // main loop
00113     pc.printf("Streaming data to address %s:%d \n\r", PC_IP, DESTINATION_PORT);
00114     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");
00115     while (true) {
00116         // tmp_addr get filled with the address of the sender of received message
00117         if(receive_sock.recvfrom(&tmp_addr, receive_buf, UDP_RECEIVE_PACKET_SIZE) > 0) {
00118             // handle received data
00119             velocities = reinterpret_cast<int32_t*>(receive_buf);
00120             pc.printf("Ricevuto da ethernet\n\r");
00121             pc.printf("%d %d \n\r", velocities[0], velocities[1]);
00122             writeCANMessages(velocities[0], velocities[1]);
00123             pc.printf("Scritto su CAN \n\r");
00124             if(enginesCAN.read(receivedCANMsg)){
00125                 pc.printf("Can loopback va \n\r");
00126             } else {
00127                 pc.printf("Can loopback NON va \n\r");
00128             }
00129             
00130         }
00131         // read from sensors
00132         magnetometer->  get_m_axes(axes_mag);
00133         accelerometer-> get_x_axes(axes_acc);
00134         gyroscope->     get_g_axes(axes_gyro);
00135         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]);
00136         send_sock.sendto(dest_addr, (void *)send_buf, 350);
00137         
00138         // Spinning at highest speed. Currently the rate is about 166Hz
00139         // wait(0.1);
00140     }
00141 }