Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1329 → Rev 1306

/demos/trunk/servo/ctrl.c
34,24 → 34,260
 
#include "servo.h"
 
void set_leg_position(double px, double py, double pz);
 
int main () {
 
int res;
char ch;
 
set_leg_position(10.0,0.0,0.0);
servo_open(COM2, 19200);
 
/*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);
 
set_leg_position(10.0,0.0,10.0);
 
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);*/
 
ch = keyb_getch(BLOCK);
 
set_leg_position(10.0,0.0,-10.0);
 
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);
 
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;
 
}
/demos/trunk/servo/makefile
12,7 → 12,7
include $(BASE)/config/example.mk
 
ctrl:
make -f $(SUBMAKE) APP=ctrl INIT= OTHEROBJS="initfile.o leg.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__"
make -f $(SUBMAKE) APP=ctrl INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__"
 
load:
make -f $(SUBMAKE) APP=load INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__"
/demos/trunk/servo/leg.c
5,14 → 5,9
 
#define LEG_A 10.0
#define LEG_B 10.0
#define LEG_C 2.0
#define LEG_D 3.0
#define LEG_CD_2IPO 2*3.603 // 2*sqrt(LEG_C^2+LEG_D^2)
#define LEG_CD_ANG 33.690 // arctg(LEG_C/LEG_D) in gradi
 
const double c0 = LEG_C * LEG_C;
const double c1 = LEG_B * LEG_B;
const double c2 = LEG_B * LEG_B - LEG_A * LEG_A;
const double c0 = LEG_A * LEG_A;
const double c1 = LEG_A * LEG_A - LEG_B * LEG_B;
const double todeg = 180.0 / PI;
 
int set_leg_position(double px, double py, double pz)
22,50 → 17,35
double py2 = py * py;
double pz2 = pz * pz;
 
double pxz2 = px2 + pz2;
double pxz2 = pz2 + px2;
double gamma1 = asin(pz/sqrt(pxz2)) * todeg;
 
int alfa,beta,gamma;
double alfa1,beta1,alfa2,beta2,gamma1;
double m,dsqrt;
double s = pxz2 + py2;
double k = c1 + s;
double k2 = k * k;
 
double delta_xz = pxz2 - c0;
double s,k,k2,y1,delta_xy;
double delta_xy = py2 * k2 - s * (k2 - 4.0 * pxz2 * c0);
double y1,dsqrt;
double alfa1,beta1,alfa2,beta2;
int alfa = 0, beta = 0, gamma = 0;
 
if (delta_xz < 0.0) return -1;
 
if (pz >= LEG_C) {
gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
} else {
gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
}
 
if (gamma1 < -90.0 || gamma1 > 90.0) return -1;
 
m = pxz2 - LEG_CD_2IPO * (sin(gamma+LEG_CD_ANG)+cos(gamma+LEG_CD_ANG));
 
s = m + py2;
k = c2 + s;
k2 = k * k;
delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1);
 
if (delta_xy >= 0.0) {
dsqrt = sqrt(delta_xy);
y1 = (py * k + dsqrt) / (2.0 * s);
beta1 = asin(y1/LEG_B) * todeg;
alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1;
alfa1 = asin(y1/LEG_A) * todeg;
beta1 = asin((y1 - py)/LEG_B) * todeg + alfa1;
y1 = (py * k - dsqrt) / (2.0 * s);
beta2 = asin(y1/LEG_B) * todeg;
alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2;
 
if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) {
alfa2 = asin(y1/LEG_A) * todeg;
beta2 = asin((y1 - py)/LEG_B) * todeg + alfa2;
cprintf("Alfa1 = %d Beta1 = %d\n",(int)alfa1,(int)beta1);
cprintf("Alfa2 = %d Beta2 = %d\n",(int)alfa2,(int)beta2);
if ((alfa1 >= -90.0 && alfa1 <= 90.0) && (beta1 >= 0.0 && beta1 <= 180.0)) {
alfa = (int)(alfa1 * 3600.0);
beta = (int)(beta1 * 3600.0);
gamma = (int)(gamma1 * 3600.0);
cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma);
return 0;
} else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
} else if ((alfa2 >= -90.0 && alfa2 <= 90.0) && (beta2 >= 0.0 && beta2 <= 180.0)) {
alfa = (int)(alfa2 * 3600.0);
beta = (int)(beta2 * 3600.0);
gamma = (int)(gamma1 * 3600.0);
75,11 → 55,9
cprintf("No possible !\n");
return 1;
}
} else
} else {
cprintf("No possible !\n");
return 1;
}
 
return 0;
 
}