Alejandro Perez / arm

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.

arm.cpp@0:ed8d2fd5fb9d, 2022-11-27 (annotated)

Committer:
aale9924
Date:
Sun Nov 27 04:35:20 2022 +0000
Revision:
0:ed8d2fd5fb9d
Initial commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aale9924 0:ed8d2fd5fb9d 1 #include "mbed.h"
aale9924 0:ed8d2fd5fb9d 2 #include "arm.h"
aale9924 0:ed8d2fd5fb9d 3
aale9924 0:ed8d2fd5fb9d 4
aale9924 0:ed8d2fd5fb9d 5 //note: can be changed but we should leave alone
aale9924 0:ed8d2fd5fb9d 6
aale9924 0:ed8d2fd5fb9d 7 //Macro function get lower 8 bits of A
aale9924 0:ed8d2fd5fb9d 8 //#define GET_LOW_BYTE(A) (uint8_t)((A))
aale9924 0:ed8d2fd5fb9d 9 //Macro function get higher 8 bits of A
aale9924 0:ed8d2fd5fb9d 10 //#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
aale9924 0:ed8d2fd5fb9d 11
aale9924 0:ed8d2fd5fb9d 12 #define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
aale9924 0:ed8d2fd5fb9d 13 #define GET_LOW_BYTE(A) (uint8_t)((A))
aale9924 0:ed8d2fd5fb9d 14
aale9924 0:ed8d2fd5fb9d 15
aale9924 0:ed8d2fd5fb9d 16
aale9924 0:ed8d2fd5fb9d 17 //initialize serial ports
aale9924 0:ed8d2fd5fb9d 18 Arm::Arm(PinName tx, PinName rx) : async_port(tx,rx){
aale9924 0:ed8d2fd5fb9d 19 //async_port(tx, rx);
aale9924 0:ed8d2fd5fb9d 20 }
aale9924 0:ed8d2fd5fb9d 21
aale9924 0:ed8d2fd5fb9d 22 void Arm::moveServo(int16_t motor, int16_t position, int16_t time){
aale9924 0:ed8d2fd5fb9d 23 // main function to move any servo at a time, parameters (ID_# (1-6) , position 0-1000)
aale9924 0:ed8d2fd5fb9d 24 char command[10];
aale9924 0:ed8d2fd5fb9d 25
aale9924 0:ed8d2fd5fb9d 26 //get motor id
aale9924 0:ed8d2fd5fb9d 27 char id = getID(motor);
aale9924 0:ed8d2fd5fb9d 28
aale9924 0:ed8d2fd5fb9d 29 command [0] = command [1] = 0x55; //start of communication
aale9924 0:ed8d2fd5fb9d 30 command [2] = 0x08; // length = 8 to control a single servo at a time
aale9924 0:ed8d2fd5fb9d 31 command [3] = 0x03; // Command: CMD_SERVO_MOVE
aale9924 0:ed8d2fd5fb9d 32 command [4] = id; // servo #
aale9924 0:ed8d2fd5fb9d 33 command [5] = GET_LOW_BYTE(time);
aale9924 0:ed8d2fd5fb9d 34 command [6] = GET_HIGH_BYTE(time);
aale9924 0:ed8d2fd5fb9d 35 command [7] = id; // servo #
aale9924 0:ed8d2fd5fb9d 36 command [8] = GET_LOW_BYTE(position);
aale9924 0:ed8d2fd5fb9d 37 command [9] = GET_HIGH_BYTE(position);
aale9924 0:ed8d2fd5fb9d 38
aale9924 0:ed8d2fd5fb9d 39 // send information to the servo
aale9924 0:ed8d2fd5fb9d 40 for(int i=0; i<10; i++){async_port.putc(command[i]);}
aale9924 0:ed8d2fd5fb9d 41
aale9924 0:ed8d2fd5fb9d 42 /*/ notes:
aale9924 0:ed8d2fd5fb9d 43 CMD_SERVO_MOVE ( command 3)
aale9924 0:ed8d2fd5fb9d 44 Command [2] or length is equal to N+2 where N is the number of parameters in bytes
aale9924 0:ed8d2fd5fb9d 45 that are sent to the servo. These 'N' parameters include command[4] to command[9].
aale9924 0:ed8d2fd5fb9d 46 */
aale9924 0:ed8d2fd5fb9d 47 }
aale9924 0:ed8d2fd5fb9d 48
aale9924 0:ed8d2fd5fb9d 49 char Arm::getID(int16_t motor){
aale9924 0:ed8d2fd5fb9d 50 char id;
aale9924 0:ed8d2fd5fb9d 51
aale9924 0:ed8d2fd5fb9d 52 switch(motor){
aale9924 0:ed8d2fd5fb9d 53 case 1:
aale9924 0:ed8d2fd5fb9d 54 id = 0x01;
aale9924 0:ed8d2fd5fb9d 55 break;
aale9924 0:ed8d2fd5fb9d 56 case 2:
aale9924 0:ed8d2fd5fb9d 57 id = 0x02;
aale9924 0:ed8d2fd5fb9d 58 break;
aale9924 0:ed8d2fd5fb9d 59 case 3:
aale9924 0:ed8d2fd5fb9d 60 id = 0x03;
aale9924 0:ed8d2fd5fb9d 61 break;
aale9924 0:ed8d2fd5fb9d 62 case 4:
aale9924 0:ed8d2fd5fb9d 63 id = 0x04;
aale9924 0:ed8d2fd5fb9d 64 break;
aale9924 0:ed8d2fd5fb9d 65 case 5:
aale9924 0:ed8d2fd5fb9d 66 id = 0x05;
aale9924 0:ed8d2fd5fb9d 67 break;
aale9924 0:ed8d2fd5fb9d 68 case 6:
aale9924 0:ed8d2fd5fb9d 69 id = 0x06;
aale9924 0:ed8d2fd5fb9d 70 break;
aale9924 0:ed8d2fd5fb9d 71 }
aale9924 0:ed8d2fd5fb9d 72 return id;
aale9924 0:ed8d2fd5fb9d 73 }