main.cpp

00001 #include "mbed.h" 
00002 
00003 Serial async_port(p9, p10); 
00004 
00005 char command [10]; // command 3 aarray
00006 char command_stop [4]; // command 6 array 
00007 
00008 // constant header value
00009 #define SERVO_FRAME_HEADER  0x55
00010 
00011 // Servo ID
00012 #define ID_1 0x01
00013 #define ID_2 0x02
00014 #define ID_3 0x03
00015 #define ID_4 0x04
00016 #define ID_5 0x05
00017 #define ID_6 0x06
00018 
00019 #define GET_LOW_BYTE(A) (uint8_t)((A))
00020 //Macro function  get lower 8 bits of A
00021 #define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
00022 //Macro function  get higher 8 bits of A
00023 
00024 void moveOneServo(char ID,int16_t position){ 
00025 // move any servo at a time, parameters (ID_# , position 0-1000)
00026  
00027     if(position < 0)
00028     position = 0;
00029     if(position > 1000)
00030     position = 1000;
00031     
00032     command [0] = command [1] = SERVO_FRAME_HEADER; 
00033     command [2] = 0x08;   // length
00034     command [3] = 0x03;   // Command
00035     command [4] = 0x01;   // # of servos
00036     command [5] = 0xD8;   // Ltime
00037     command [6] = 0x07;   // Htime
00038     command [7] = ID;     // ID
00039     command [8] = GET_LOW_BYTE(position);
00040     command [9] = GET_HIGH_BYTE(position);
00041     
00042     for(int i=0; i<10; i++){async_port.putc(command[i]);} 
00043     
00044 /*/ notes: 
00045     CMD_SERVO_MOVE ( command 3) 
00046 */
00047 }
00048 
00049 int main() {
00050     
00051 // note: wait should be 5s to allow the servo enough time to position itself
00052 
00053     async_port.baud(9600); // boud rate provided by user manual
00054 
00055     while(1) { 
00056     
00057         moveOneServo(ID_3,0); 
00058         wait(5); 
00059         moveOneServo(ID_3,1000);
00060         wait(5);
00061         
00062 //Testing
00063         ////////////////////////
00064 /*
00065         closeGrabber();
00066         wait(1); 
00067         //stopProgram();
00068         //wait(1); 
00069         openGrabber();
00070         //wait(1); 
00071         //stopProgram();
00072         wait(1);     
00073         ////////////////////////
00074         for(int i=0; i<10; i++){async_port.putc(close_grabber[i]);} 
00075         wait(1); 
00076         for(int i=0; i<10; i++){async_port.putc(stop_prog[i]);}
00077         wait(1); 
00078         for(int i=0; i<10; i++){async_port.putc(open_grabber[i]);}
00079         wait(1); 
00080         for(int i=0; i<10; i++){async_port.putc(stop_prog[i]);}
00081         wait(1); 
00082 */
00083     } 
00084 
00085 } 
00086 void closeGrabber(){ // function to only close grabber
00087     
00088     command [0] = command [1] = SERVO_FRAME_HEADER; 
00089     command [2] = 0x08;
00090     command [3] = 0x03;
00091     command [4] = 0x01;
00092     command [5] = 0xE8;
00093     command [6] = 0x03;
00094     command [7] = 0x01;
00095     command [8] = 0xD8;
00096     command [9] = 0x07;
00097     
00098     for(int i=0; i<10; i++){async_port.putc(command[i]);} 
00099     
00100 /*/ notes: 
00101     CMD_SERVO_MOVE ( command 3) 
00102 */
00103 }
00104 void openGrabber(){// function to only open grabber
00105     
00106     command [0] = command [1] = SERVO_FRAME_HEADER; 
00107     command [2] = 0x08;
00108     command [3] = 0x03;
00109     command [4] = 0x01;
00110     command [5] = 0xE8;
00111     command [6] = 0x03;
00112     command [7] = 0x01;
00113     command [8] = 0x00;
00114     command [9] = 0x00;
00115     
00116     for(int i=0; i<10; i++){async_port.putc(command[i]);} 
00117 /*/ notes: 
00118     CMD_SERVO_MOVE ( command 3) 
00119 */
00120 }
00121 void stopProgram(){// function to stop all programs
00122     
00123     command [0] = command [1] = SERVO_FRAME_HEADER; 
00124     command [2] = 0x02;
00125     command [3] = 0x07;
00126     
00127     for(int i=0; i<10; i++){async_port.putc(command[i]);} 
00128     
00129 /*/ notes: 
00130     CMD_ACTION_STOP( command 6) 
00131 */
00132     
00133 }