LacqueyFetch firmware for the mbed

Dependencies:   Motor mbed

Committer:
raulsr988
Date:
Tue Mar 14 10:09:40 2023 +0000
Revision:
4:f27d1844c17a
Parent:
3:f31b6ddb9cd3
Update pin assignment for MBED/Lacquey

Who changed what in which revision?

UserRevisionLine numberNew contents of line
raulsr988 0:f8a72cc60181 1 /**
raulsr988 0:f8a72cc60181 2 *
raulsr988 0:f8a72cc60181 3 * @ingroup teo_body_lacqueyFetch
raulsr988 0:f8a72cc60181 4 * \defgroup teo_body_MbedFirmware Mbed-Firmware
raulsr988 0:f8a72cc60181 5 *
raulsr988 0:f8a72cc60181 6 * @brief Firmware that allows to recieve CAN messages to open and close the hand
raulsr988 0:f8a72cc60181 7 *
raulsr988 0:f8a72cc60181 8 * @section teo_body_firmware_legal Legal
raulsr988 0:f8a72cc60181 9 *
raulsr988 0:f8a72cc60181 10 * Copyright: 2018 (C) Universidad Carlos III de Madrid
raulsr988 0:f8a72cc60181 11 *
raulsr988 0:f8a72cc60181 12 * Author: <a href="http://roboticslab.uc3m.es/roboticslab/people/r-de-santos">Raul de Santos Rico</a>
raulsr988 0:f8a72cc60181 13 *
raulsr988 0:f8a72cc60181 14 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see license/LGPL.TXT
raulsr988 0:f8a72cc60181 15 *
raulsr988 0:f8a72cc60181 16 * @section lacqueyFetch_install Installation
raulsr988 0:f8a72cc60181 17 *
raulsr988 1:3b3e8f8352a8 18 * First you need to program the firmware to the MBED <br>
raulsr988 4:f27d1844c17a 19 * You can use the online compiler (https://studio.keil.arm.com) to compile the firmware and stored it on the flash mbed memory<br>
raulsr988 0:f8a72cc60181 20 * Steps:
raulsr988 0:f8a72cc60181 21 * - Change the "id" of the device corresponding to the hand used (right-hand: ID 65, left-hand: ID 64) <br>
raulsr988 1:3b3e8f8352a8 22 * - Press "Compile" and save the .bin file on the flash mbed memory <br>
raulsr988 3:f31b6ddb9cd3 23
raulsr988 0:f8a72cc60181 24 * - 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>
raulsr988 0:f8a72cc60181 25 *
raulsr988 0:f8a72cc60181 26 *
raulsr988 1:3b3e8f8352a8 27 * @section lacqueyFetch_running Running (assuming correct installation)
raulsr988 0:f8a72cc60181 28 *
raulsr988 1:3b3e8f8352a8 29 * Running "launchManipulation" application, you can open or close the hand with this parameters (assuming you know how to run "launchManipulation"): <br>
raulsr988 2:8ebb11ba21be 30 * set pos 6 1200 -> open hand <br>
raulsr988 2:8ebb11ba21be 31 * set pos 6 -1200 -> close hand <br>
raulsr988 2:8ebb11ba21be 32 * set pos 6 0 -> loose hand <br>
raulsr988 0:f8a72cc60181 33 *
raulsr988 0:f8a72cc60181 34 **/
raulsr988 0:f8a72cc60181 35
raulsr988 0:f8a72cc60181 36 #include "mbed.h"
raulsr988 0:f8a72cc60181 37 #include "Motor.h"
raulsr988 0:f8a72cc60181 38
raulsr988 0:f8a72cc60181 39 Serial pc(USBTX, USBRX); // Serial connection
raulsr988 0:f8a72cc60181 40 DigitalOut led1(LED1); // received
raulsr988 0:f8a72cc60181 41 CAN can(p30, p29); // tx,rx
raulsr988 4:f27d1844c17a 42 Motor m(p22, p23, p24); // pwm, fwd, rev (PWM, REVERSE, FORDWARD)
raulsr988 3:f31b6ddb9cd3 43 const uint8_t id = 64; // right-hand: ID 65, left-hand: ID 64
raulsr988 0:f8a72cc60181 44
raulsr988 3:f31b6ddb9cd3 45 bool receive(float* value) {
raulsr988 0:f8a72cc60181 46 CANMessage msg;
raulsr988 3:f31b6ddb9cd3 47
raulsr988 3:f31b6ddb9cd3 48 if( can.read(msg) ) {
raulsr988 3:f31b6ddb9cd3 49 if(id == (uint8_t)msg.id) {
raulsr988 3:f31b6ddb9cd3 50 pc.printf("Message received (ID: %d) (size: %d)\n", (int8_t)msg.id, msg.len);
raulsr988 3:f31b6ddb9cd3 51
raulsr988 3:f31b6ddb9cd3 52 if(msg.len == 4) {
raulsr988 3:f31b6ddb9cd3 53 pc.printf("Message received %x %x %x %x\n", msg.data[0], msg.data[1], msg.data[2], msg.data[3]);
raulsr988 3:f31b6ddb9cd3 54 *value = *((float*)msg.data);
raulsr988 0:f8a72cc60181 55 led1 = !led1;
raulsr988 0:f8a72cc60181 56 return true;
raulsr988 0:f8a72cc60181 57 }
raulsr988 3:f31b6ddb9cd3 58 }
raulsr988 3:f31b6ddb9cd3 59 }
raulsr988 0:f8a72cc60181 60
raulsr988 0:f8a72cc60181 61 return false;
raulsr988 0:f8a72cc60181 62 }
raulsr988 0:f8a72cc60181 63
raulsr988 0:f8a72cc60181 64 int main() {
raulsr988 0:f8a72cc60181 65 // set up the serial usb connection
raulsr988 0:f8a72cc60181 66 pc.baud(115200); //-- configure PC baudrate
raulsr988 0:f8a72cc60181 67 pc.printf("main()\n");
raulsr988 0:f8a72cc60181 68 // set up the can bus
raulsr988 0:f8a72cc60181 69 can.frequency(1000000); //-- 1Mbit/s (the same as the drivers configuration)
raulsr988 0:f8a72cc60181 70 can.reset();
raulsr988 0:f8a72cc60181 71
raulsr988 0:f8a72cc60181 72 // message received
raulsr988 3:f31b6ddb9cd3 73 float data;
raulsr988 0:f8a72cc60181 74
raulsr988 3:f31b6ddb9cd3 75 while(1) {
raulsr988 0:f8a72cc60181 76 if(receive(&data)) {
raulsr988 3:f31b6ddb9cd3 77 pc.printf("value received: %5.2f\n", data);
raulsr988 3:f31b6ddb9cd3 78
raulsr988 3:f31b6ddb9cd3 79 if (data >= -100.0f && data <= 100.0f) {
raulsr988 3:f31b6ddb9cd3 80 m.speed(data/100.0f);
raulsr988 3:f31b6ddb9cd3 81 }
raulsr988 0:f8a72cc60181 82 }
raulsr988 0:f8a72cc60181 83 }
raulsr988 0:f8a72cc60181 84 }