Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1213 → Rev 1215

/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