arm.cpp

00001 #include "mbed.h"
00002 #include "arm.h"
00003 
00004 
00005 //note: can be changed but we should leave alone 
00006 
00007 //Macro function  get lower 8 bits of A
00008 //#define GET_LOW_BYTE(A) (uint8_t)((A))
00009 //Macro function  get higher 8 bits of A
00010 //#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
00011 
00012 #define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
00013 #define GET_LOW_BYTE(A) (uint8_t)((A))
00014 
00015 
00016 
00017 //initialize serial ports
00018 Arm::Arm(PinName tx, PinName rx) : async_port(tx,rx){
00019     //async_port(tx, rx);
00020 }
00021 
00022 void Arm::moveServo(int16_t motor, int16_t position, int16_t time){ 
00023 // main function to move any servo at a time, parameters (ID_# (1-6) , position 0-1000)
00024     char command[10];
00025     
00026     //get motor id
00027     char id = getID(motor);
00028     
00029     command [0] = command [1] = 0x55; //start of communication 
00030     command [2] = 0x08;   // length = 8 to control a single servo at a time
00031     command [3] = 0x03;   // Command: CMD_SERVO_MOVE
00032     command [4] = id;   // servo #
00033     command [5] = GET_LOW_BYTE(time);
00034     command [6] = GET_HIGH_BYTE(time);
00035     command [7] = id;     // servo #
00036     command [8] = GET_LOW_BYTE(position);
00037     command [9] = GET_HIGH_BYTE(position);
00038     
00039     // send information to the servo
00040     for(int i=0; i<10; i++){async_port.putc(command[i]);} 
00041     
00042 /*/ notes: 
00043     CMD_SERVO_MOVE ( command 3)
00044     Command [2] or length is equal to N+2 where N is the number of parameters in bytes
00045     that are sent to the servo. These 'N' parameters include command[4] to command[9].
00046 */
00047 }
00048 
00049  char Arm::getID(int16_t motor){
00050     char id;
00051     
00052     switch(motor){
00053         case 1:
00054             id = 0x01;
00055             break;
00056         case 2:
00057             id = 0x02;
00058             break;
00059         case 3:
00060             id = 0x03;
00061             break;
00062         case 4:
00063             id = 0x04;
00064             break;
00065         case 5:
00066             id = 0x05;
00067             break;
00068         case 6:
00069             id = 0x06;
00070             break;
00071     }
00072     return id;
00073 }