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; |