| /demos/trunk/servo/servo.c |
|---|
| 3,14 → 3,15 |
| #include "drivers/keyb.h" |
| #include "servo.h" |
| #include "leg.h" |
| int main () { |
| int res; |
| char ch; |
| /* |
| servo_open(SERVO_COM2); |
| servo_open(SERVO_COM2); |
| /* |
| cprintf("servo_set_RS232_baudrate\n"); |
| res = servo_set_RS232_baudrate(9600); |
| cprintf("Res = %d\n",res); |
| 22,7 → 23,7 |
| cprintf("servo_store_RS232_baudrate\n"); |
| res = servo_store_RS232_baudrate(); |
| cprintf("Res = %d\n",res); |
| */ |
| cprintf("servo_set_period\n"); |
| res = servo_set_period(20000); |
| cprintf("Res = %d\n",res); |
| 78,7 → 79,7 |
| ch = keyb_getch(BLOCK); |
| cprintf("servo_set_levels\n"); |
| res = servo_set_levels(0x01); |
| res = servo_set_levels(0x00,0x01); |
| cprintf("Res = %d\n",res); |
| ch = keyb_getch(BLOCK); |
| 120,6 → 121,9 |
| ch = keyb_getch(BLOCK); |
| servo_close(); |
| */ |
| set_leg_position(8.0,-5.0,0.0); |
| set_leg_position(8.0,0.0,0.0); |
| return 0; |
| /demos/trunk/servo/initfile.c |
|---|
| 57,7 → 57,7 |
| struct multiboot_info *mb = (struct multiboot_info *)arg; |
| EDF_register_level(EDF_ENABLE_ALL); |
| CBS_register_level(CBS_ENABLE_ALL, 0); |
| //CBS_register_level(CBS_ENABLE_ALL, 0); |
| RR_register_level(RRTICK, RR_MAIN_YES, mb); |
| dummy_register_level(); |
| 74,7 → 74,7 |
| HARTPORT_init(); |
| KEYB_init(NULL); |
| //KEYB_init(NULL); |
| __call_main__(mb); |
| /demos/trunk/servo/leg.c |
|---|
| 0,0 → 1,63 |
| #include <kernel/kern.h> |
| #include <math.h> |
| #include "servo.h" |
| #define LEG_A 10.0 |
| #define LEG_B 10.0 |
| 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) |
| { |
| double px2 = px * px; |
| double py2 = py * py; |
| double pz2 = pz * pz; |
| double pxz2 = pz2 + px2; |
| double gamma1 = asin(pz/sqrt(pxz2)) * todeg; |
| double s = pxz2 + py2; |
| double k = c1 + s; |
| double k2 = k * k; |
| 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_xy >= 0.0) { |
| dsqrt = sqrt(delta_xy); |
| y1 = (py * k + dsqrt) / (2.0 * s); |
| alfa1 = asin(y1/LEG_A) * todeg; |
| beta1 = asin((y1 - py)/LEG_B) * todeg + alfa1; |
| y1 = (py * k - dsqrt) / (2.0 * s); |
| 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 >= -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); |
| cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma); |
| return 0; |
| } else { |
| cprintf("No possible !\n"); |
| return 1; |
| } |
| } else { |
| cprintf("No possible !\n"); |
| return 1; |
| } |
| } |
| /demos/trunk/servo/makefile |
|---|
| 12,5 → 12,5 |
| include $(BASE)/config/example.mk |
| servo: |
| make -f $(SUBMAKE) APP=servo INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__" |
| make -f $(SUBMAKE) APP=servo INIT= OTHEROBJS="initfile.o leg.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__" |
| /demos/trunk/servo/leg.h |
|---|
| 0,0 → 1,6 |
| #ifndef __LEG_H__ |
| #define __LEG_H__ |
| int set_leg_position(double px, double py, double pz); |
| #endif |