File content as of revision 0:ed8d2fd5fb9d:

#include "mbed.h"
#include "arm.h"


//note: can be changed but we should leave alone 

//Macro function  get lower 8 bits of A
//#define GET_LOW_BYTE(A) (uint8_t)((A))
//Macro function  get higher 8 bits of A
//#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)

#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
#define GET_LOW_BYTE(A) (uint8_t)((A))



//initialize serial ports
Arm::Arm(PinName tx, PinName rx) : async_port(tx,rx){
    //async_port(tx, rx);
}

void Arm::moveServo(int16_t motor, int16_t position, int16_t time){ 
// main function to move any servo at a time, parameters (ID_# (1-6) , position 0-1000)
    char command[10];
    
    //get motor id
    char id = getID(motor);
    
    command [0] = command [1] = 0x55; //start of communication 
    command [2] = 0x08;   // length = 8 to control a single servo at a time
    command [3] = 0x03;   // Command: CMD_SERVO_MOVE
    command [4] = id;   // servo #
    command [5] = GET_LOW_BYTE(time);
    command [6] = GET_HIGH_BYTE(time);
    command [7] = id;     // servo #
    command [8] = GET_LOW_BYTE(position);
    command [9] = GET_HIGH_BYTE(position);
    
    // send information to the servo
    for(int i=0; i<10; i++){async_port.putc(command[i]);} 
    
/*/ notes: 
    CMD_SERVO_MOVE ( command 3)
    Command [2] or length is equal to N+2 where N is the number of parameters in bytes
    that are sent to the servo. These 'N' parameters include command[4] to command[9].
*/
}

 char Arm::getID(int16_t motor){
    char id;
    
    switch(motor){
        case 1:
            id = 0x01;
            break;
        case 2:
            id = 0x02;
            break;
        case 3:
            id = 0x03;
            break;
        case 4:
            id = 0x04;
            break;
        case 5:
            id = 0x05;
            break;
        case 6:
            id = 0x06;
            break;
    }
    return id;
}