Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1636 → Rev 1637

/advdemos/trunk/chimera/chimera.c
31,8 → 31,6
kern_printf("Task Name : %s - Max Time : %d\n", proc_table[i].name, (int)tmp);
}
 
trace_send();
 
sys_end();
 
}
68,7 → 66,7
 
TASK walk_auto_6()
{
short i, n;
short i, n = 0;
int vd, vs, auto_count = 0;
int pos_a[6], pos_b[6], pos_c[6];
struct action_event e;
248,7 → 246,7
hard_task_default_model(ms);
hard_task_def_ctrl_jet(ms);
hard_task_def_wcet(ms, 1000);
hard_task_def_mit(ms, 30000);
hard_task_def_mit(ms, 60000);
hard_task_def_usemath(ms);
pid_walk = task_create("Walk_Task", walk_auto_3, &ms, NULL);
if (pid_walk == NIL) {
273,19 → 271,11
 
clear();
 
trace_init(1000000);
 
init_send(); /* Comunication */
init_key(); /* Keyboard */
 
init_action_event(100);
 
if (argc < 2) sys_end();
 
beta = atoi(argv[1]);
cprintf("Beta = %d\n",beta);
 
//action_stand_up();
action_walk();
 
return 0;
/advdemos/trunk/chimera/send.c
11,8 → 11,6
*/
 
#include "chimera.h"
#include "tracer.h"
#include "unistd.h"
 
//#define DEBUG_SEND /* Print Sent Values */
#define SERIAL_ON /* Send Data using Serial Port */
19,11 → 17,11
 
/* Servo Tasks Constants */
#ifdef DUBUG_SEND
#define SEND_TASK_WCET 26000
#define SEND_TASK_WCET 1000
#else
#define SEND_TASK_WCET 26000
#define SEND_TASK_WCET 1000
#endif
#define SEND_TASK_MIT 30000
#define SEND_TASK_MIT 10000
 
#define LEG_A 100.0
#define LEG_B 66.0
40,15 → 38,8
 
HEXAPOD_STATE status;
 
void *start_tracer = NULL;
void *end_tracer = NULL;
void *trace_pointer = NULL;
 
extern struct leg_calibration calibration_table[];
 
void print_status(int n){
}
 
int leg_to_ang(float px, float py, float pz, int *alfa, int *beta, int *gamma)
{
float px2 = px * px;
164,137 → 155,11
 
}
 
int trace_init(int buffer_size)
{
 
start_tracer = trace_pointer = (void *)malloc(buffer_size);
end_tracer = start_tracer + buffer_size;
memset(trace_pointer,0,buffer_size);
 
if (trace_pointer == NULL) return -1;
 
return 0;
}
 
/* Init the network stack */
int init_network(char *local_ip)
{
struct net_model m = net_base;
net_setudpip(m, local_ip, "255.255.255.255");
if (net_init(&m) != 1) {
cprintf("Network: Init Error.\n");
return -1;
}
return 0;
}
 
int trace_consumption(int sensor, int value)
{
 
struct timespec t;
SYS_FLAGS f;
 
if (trace_pointer == NULL) return -1;
 
if (trace_pointer >= (end_tracer-16)) return -1;
 
f = kern_fsave();
 
sys_gettime(&t);
 
*(unsigned int *)trace_pointer = (sensor & 0xFF) | 0xAABBFF00;
*(unsigned int *)(trace_pointer + 4) = value;
*(unsigned int *)(trace_pointer + 8) = t.tv_sec;
*(unsigned int *)(trace_pointer + 12) = t.tv_nsec;
 
trace_pointer += 16;
 
kern_frestore(f);
 
return 0;
 
}
 
int trace_send() {
 
static char pkg_buffer[1100];
int actual = 0;
 
UDP_ADDR target, local;
char local_ip[20], target_ip[20];
int socket;
IP_ADDR bindlist[5];
strcpy(local_ip, "192.168.0.99");
strcpy(target_ip, "192.168.0.104");
if (init_network(local_ip)) sys_end();
/* local IP string to addr */
ip_str2addr(local_ip,&(local.s_addr));
/* set the source port */
local.s_port = 20000;
/* target IP string to addr */
ip_str2addr(target_ip,&(bindlist[0]));
memset(&(bindlist[1]), 0, sizeof(IP_ADDR));
/* bind */
socket = udp_bind(&local, NULL);
/* target IP string to addr */
ip_str2addr(target_ip,&(target.s_addr));
/* target port */
target.s_port = 20000;
 
trace_pointer = start_tracer;
actual = 0;
 
while(trace_pointer < end_tracer) {
 
if (((*(int *)(trace_pointer) >> 8) & 0xAABBFF) == 0xAABBFF) {
 
cprintf(".");
 
memcpy(&(pkg_buffer[actual]),trace_pointer,16);
 
actual += 16;
 
if (actual > 800) {
pkg_buffer[actual] = 0;
cprintf("X");
udp_sendto(socket, pkg_buffer, actual, &target);
usleep(100000);
actual = 0;
}
 
}
 
trace_pointer += 16;
 
}
 
pkg_buffer[actual] = 0;
cprintf("X");
udp_sendto(socket, pkg_buffer, actual+1, &target);
usleep(10000);
actual = 0;
 
return 0;
 
}
 
TASK servo_send()
{
HEXAPOD_STATE old_status;
register char new_pos, new_pwm, new_power;
int res,n;
struct timespec t;
int actual_leg = 0;
 
for (n=0; n<6;n++) {
old_status.ang[n].a = 0;
321,25 → 186,13
}
task_preempt();
old_status.power = status.power;
 
task_endcycle();
 
#endif
}
 
for (n=0; n<6; n++){
#ifdef SERIAL_ON
task_nopreempt();
status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg);
trace_consumption(actual_leg,status.cfg[actual_leg].adc_in);
task_preempt();
#endif
sys_gettime(&t);
printf_xy(1,20,WHITE,"(%d) (%d) (%d) (%d) (%d) (%d) ",
status.cfg[0].adc_in,
status.cfg[1].adc_in,
status.cfg[2].adc_in,
status.cfg[3].adc_in,
status.cfg[4].adc_in,
status.cfg[5].adc_in);
actual_leg = (actual_leg+1)%6;
 
if ((status.ang[n].a != old_status.ang[n].a) ||
(status.ang[n].b != old_status.ang[n].b) ||
361,11 → 214,24
task_nopreempt();
res = servo_set_angle_sec(com(n*3 ), pin(n*3 ), adjust(status.ang[n].a,n,0));
if (res != 0) cprintf("Error send data\n");
task_preempt();
 
task_endcycle();
 
task_nopreempt();
res = servo_set_angle_sec(com(n*3+1), pin(n*3+1), adjust(status.ang[n].b,n,1));
if (res != 0) cprintf("Error send data\n");
task_preempt();
 
task_endcycle();
task_nopreempt();
res = servo_set_angle_sec(com(n*3+2), pin(n*3+2), adjust(status.ang[n].c,n,2));
if (res != 0) cprintf("Error send data\n");
task_preempt();
 
task_endcycle();
 
#endif
 
}
374,15 → 240,28
#ifdef SERIAL_ON
task_nopreempt();
(old_status.cfg[n].pwm & 1) ? servo_turn_on(com(n*3 ), pin(n*3 )) : servo_turn_off(com(n*3 ), pin(n*3 ));
task_preempt();
 
task_endcycle();
 
task_nopreempt();
(old_status.cfg[n].pwm & 2) ? servo_turn_on(com(n*3+1), pin(n*3+1)) : servo_turn_off(com(n*3+1), pin(n*3+1));
task_preempt();
 
task_endcycle();
 
task_nopreempt();
(old_status.cfg[n].pwm & 4) ? servo_turn_on(com(n*3+2), pin(n*3+2)) : servo_turn_off(com(n*3+2), pin(n*3+2));
task_preempt();
task_endcycle();
 
#endif
}
}
 
task_endcycle();
task_testcancel();
task_endcycle();
}
return 0;
/advdemos/trunk/chimera/chimera.h
23,7 → 23,7
#include <drivers/keyb.h>
 
#include <servo.h>
#include <drivers/udpip.h>
//#include <drivers/udpip.h>
 
/* COM Port Constants */
#define COM_PORT COM1