main.cpp

00001 /**
00002  *
00003  * @ingroup teo_body_lacqueyFetch
00004  * \defgroup teo_body_MbedFirmware Mbed-Firmware
00005  *
00006  * @brief Firmware that allows to recieve CAN messages to open and close the hand 
00007  *
00008  * @section teo_body_firmware_legal Legal
00009  *
00010  * Copyright: 2018 (C) Universidad Carlos III de Madrid
00011  *
00012  * Author: <a href="http://roboticslab.uc3m.es/roboticslab/people/r-de-santos">Raul de Santos Rico</a>
00013  *
00014  * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see license/LGPL.TXT
00015  *
00016  * @section lacqueyFetch_install Installation
00017  * 
00018  * First you need to program the firmware to the MBED <br>
00019  * You can use the online compiler (https://studio.keil.arm.com) to compile the firmware and stored it on the flash mbed memory<br>
00020  * Steps:
00021  * - Change the "id" of the device corresponding to the hand used (right-hand: ID 65, left-hand: ID 64) <br>
00022  * - Press "Compile" and save the .bin file on the flash mbed memory <br>
00023  
00024  * - If you want to check the correct functionality, you can connect the mbed to the serial USB and with a serial monitor program (configured with a baudrate of 115200), see the CAN messages information of the hand. <br>
00025  *
00026  *
00027  * @section lacqueyFetch_running Running (assuming correct installation) 
00028  *
00029  * Running "launchManipulation" application, you can open or close the hand with this parameters (assuming you know how to run "launchManipulation"): <br>
00030  * set pos 6 1200   -> open hand  <br>
00031  * set pos 6 -1200  -> close hand <br>
00032  * set pos 6 0      -> loose hand <br>  
00033  *
00034  **/
00035  
00036 #include "mbed.h"
00037 #include "Motor.h"
00038 
00039 Serial pc(USBTX, USBRX);   // Serial connection
00040 DigitalOut led1(LED1);     // received
00041 CAN can(p30, p29);         // tx,rx
00042 Motor m(p22, p23, p24);    // pwm, fwd, rev (PWM, REVERSE, FORDWARD)
00043 const uint8_t id = 64;     // right-hand: ID 65, left-hand: ID 64
00044 
00045 bool receive(float* value) {
00046     CANMessage msg;
00047     
00048     if( can.read(msg) ) {                                     
00049         if(id == (uint8_t)msg.id) {
00050             pc.printf("Message received (ID: %d) (size: %d)\n", (int8_t)msg.id, msg.len);
00051 
00052             if(msg.len == 4) {
00053                 pc.printf("Message received %x %x %x %x\n", msg.data[0], msg.data[1], msg.data[2], msg.data[3]);
00054                 *value = *((float*)msg.data);
00055                 led1 = !led1;
00056                 return true;
00057             }
00058         }
00059     } 
00060         
00061     return false;    
00062 }
00063 
00064 int main() {
00065     // set up the serial usb connection
00066     pc.baud(115200);          //-- configure PC baudrate
00067     pc.printf("main()\n");     
00068     // set up the can bus
00069     can.frequency(1000000);   //-- 1Mbit/s (the same as the drivers configuration)
00070     can.reset(); 
00071     
00072     // message received
00073     float data;    
00074    
00075     while(1) {  
00076         if(receive(&data)) {
00077             pc.printf("value received: %5.2f\n", data);
00078             
00079             if (data >= -100.0f && data <= 100.0f) {
00080                 m.speed(data/100.0f);
00081             }
00082         }  
00083     }
00084 }