34,260 → 34,24 |
|
#include "servo.h" |
|
void set_leg_position(double px, double py, double pz); |
|
int main () { |
|
int res; |
char ch; |
|
servo_open(COM2, 19200); |
set_leg_position(10.0,0.0,0.0); |
|
/*cprintf("servo_set_RS232_baudrate\n"); |
res = servo_set_RS232_baudrate(COM2, 115200); |
cprintf("Res = %d\n",res);*/ |
|
cprintf("servo_get_RS232_baudrate\n"); |
res = servo_get_RS232_baudrate(COM2); |
cprintf("Res = %d\n",res); |
|
/*cprintf("servo_store_RS232_baudrate\n"); |
res = servo_store_RS232_baudrate(COM2); |
cprintf("Res = %d\n\n",res);*/ |
|
ch = keyb_getch(BLOCK); |
|
|
cprintf("servo_set_period\n"); |
res = servo_set_period(COM2, 20000); |
cprintf("Res = %d\n",res); |
|
cprintf("servo_get_period\n"); |
res = servo_get_period(COM2); |
cprintf("Res = %d\n",res); |
|
/*cprintf("servo_store_period\n"); |
res = servo_store_period(COM2); |
cprintf("Res = %d\n\n",res);*/ |
|
set_leg_position(10.0,0.0,10.0); |
|
ch = keyb_getch(BLOCK); |
|
|
cprintf("servo_get_setup_switch\n"); |
res = servo_get_setup_switch(COM2); |
cprintf("Res = %d\n",res); |
|
cprintf("servo_set_RC5_switch\n"); |
res = servo_set_RC5_switch(COM2, 1); |
cprintf("Res = %d\n\n",res); |
|
set_leg_position(10.0,0.0,-10.0); |
|
ch = keyb_getch(BLOCK); |
|
|
cprintf("servo_set_levels\n"); |
res = servo_set_levels(COM2, 0x00,0x01); |
cprintf("Res = %d\n",res); |
|
cprintf("servo_get_levels\n"); |
res = servo_get_levels(COM2, 1); |
cprintf("Res = %d\n\n",res); |
|
ch = keyb_getch(BLOCK); |
|
cprintf("servo_turn_off\n"); |
res = servo_turn_off(COM2, 1); |
cprintf("Res = %d\n\n",res); |
|
ch = keyb_getch(BLOCK); |
|
cprintf("servo_turn_on\n"); |
res = servo_turn_on(COM2, 1); |
cprintf("Res = %d\n\n",res); |
|
ch = keyb_getch(BLOCK); |
|
cprintf("servo_turn_off_all\n"); |
res = servo_turn_off_all(COM2); |
cprintf("Res = %d\n\n",res); |
|
ch = keyb_getch(BLOCK); |
|
cprintf("servo_turn_on_all\n"); |
res = servo_turn_on_all(COM2); |
cprintf("Res = %d\n\n",res); |
|
ch = keyb_getch(BLOCK); |
|
/*cprintf("servo_store_levels\n"); |
res = servo_store_levels(COM2); |
cprintf("Res = %d\n\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
|
cprintf("servo_get_analog\n"); |
res = servo_get_analog(COM2, 0); |
cprintf("Res = %d\n\n",res); |
|
ch = keyb_getch(BLOCK); |
|
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 0,ANGLE2SEC(45,0,0)); |
cprintf("Res = %d\n",res); |
|
cprintf("servo_get_angle_sec\n"); |
res = servo_get_angle_sec(COM2, 0); |
cprintf("Res = %d\n\n",res); |
|
ch = keyb_getch(BLOCK); |
|
/*----------------------------------------- */ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 0,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 0); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK); */ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 1,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 0); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK); */ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 2,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 2); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK); */ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 3,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 3); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK); */ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 4,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 4); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK); */ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 5,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 5); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK); */ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 6,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 6); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK); */ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 7,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 7); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 8,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 8); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 9,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 9); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 10,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 10); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 11,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 11); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 12,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 12); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 13,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 13); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 14,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 14); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
cprintf("servo_set_angle_sec\n"); |
res = servo_set_angle_sec(COM2, 15,ANGLE2SEC(0,0,0)); |
cprintf("Res = %d\n",res); |
/*cprintf("servo_store_default_position\n"); |
res = servo_store_default_position(COM2, 15); |
cprintf("Res = %d\n",res); |
|
ch = keyb_getch(BLOCK);*/ |
|
servo_close(COM2); |
|
return 0; |
|
} |