int main(int argc, char *argv[]) {
setup_gpio_address();
INP_GPIO(GPIO_PIN);
OUT_GPIO(GPIO_PIN);
mlockall(MCL_CURRENT|MCL_FUTURE);
//servo1 = open("/sys/class/gpio/gpio18/value",O_WRONLY);
int err = rt_task_create(&task_desc, "MiseA1", TASK_STKSZ, TASK_PRIO, TASK_MODE);
err |= rt_alarm_create(&alarm_desc,"MiseA0");
err |= rt_task_set_periodic(&task_desc, TM_NOW, 20000000);
if (!err) {
rt_task_start(&task_desc,&task_body,NULL);
}
pause();
return 0;
}
void fonction_periodique (void * arg)
{
RTIME precedent = 0;
RTIME heure = 0;
RTIME periode;
RTIME duree;
periode = * (RTIME *) arg;
periode = periode * 1000; // en ns
rt_task_set_periodic(NULL, TM_NOW, periode);
while(1) {
rt_task_wait_period(NULL);
heure = rt_timer_read();
// ignorer le premier declenchement
if (precedent != 0) {
duree = heure - precedent;
rt_printf("%llu\n", duree/1000);
}
precedent = heure;
}
}
void lMotor_stop(void *arg)
{
// rt_task_delete(&rMotor_task);
int fd;
char buf[MAX_BUF];
RTIME now, previous;
long MAX = 0;
int flag = 1;
//Turn off
snprintf(buf, sizeof(buf), "/sys/devices/ocp.3/pwm_test_P9_22.13/duty");
rt_task_set_periodic(NULL, TM_NOW, period_motorStop);
rt_printf("Running left motor STOP task!\n");
while (1){
rt_task_wait_period(NULL);
previous = rt_timer_read();
if(flagStop){
rt_task_delete(&lMotor_task);
fd = open(buf, O_RDWR);
if(fd < 0){
perror("Problem opening Duty file - leeft motor");
}
//Duty 0 to stop
write(fd, "0", 5);
close(fd);
rt_printf("LMotor stopped \n");
now = rt_timer_read();
if (flag){
MAX = now- previous;
flag = 0;
}
if((long)((now - previous)) > MAX){
MAX = (long)((now - previous)) ;
}
rt_printf("WCET Left Motor Stop: %ld \n", MAX);
}
}
}
void LinearMotorControlTask::run()
{
can_frame startBeckhoffFrame;
startBeckhoffFrame.can_id = 0x0;
startBeckhoffFrame.can_dlc = 2;
startBeckhoffFrame.data[0] = 1;
startBeckhoffFrame.data[1] = 1;
can->sendFrame(startBeckhoffFrame);
can_frame answerFrame;
rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(1000000));
while (1)
{
can->sendFrame(pdoOneFrame);
can->sendFrame(pdoTwoFrame);
can->sendFrame(pdoThreeFrame);
if (sendStates)
{
can->sendFrame(stateOneFrame);
can->sendFrame(stateTwoFrame);
can->sendFrame(stateThreeFrame);
sendStates = false;
}
for (int i = 0; i < 3; ++i)
{
can->recvFrame(answerFrame);
answerFrameMap[answerFrame.can_id] = answerFrame;
}
/*setAccelerationOne((uint32_t)((double)getAccelerationSetpointOne()*tanh((double)abs(getPositionSetpointOne()-getPositionOne()))));
setAccelerationTwo((uint32_t)((double)getAccelerationSetpointTwo()*tanh((double)abs(getPositionSetpointTwo()-getPositionTwo()))));
setAccelerationThree((uint32_t)((double)getAccelerationSetpointThree()*tanh((double)abs(getPositionSetpointThree()-getPositionThree()))));*/
/*setAccelerationOne((uint32_t)(accUpperBound*tanh((double)abs(getPositionSetpointOne()-getPositionOne())*0.001)));
setAccelerationTwo((uint32_t)(accUpperBound*tanh((double)abs(getPositionSetpointTwo()-getPositionTwo())*0.001)));
setAccelerationThree((uint32_t)(accUpperBound*tanh((double)abs(getPositionSetpointThree()-getPositionThree())*0.001)));*/
rt_task_wait_period(&overruns);
}
}
void executor(void *args){
rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
short step = (short)*args;
actuators = 0; // how to initialize the temporary value of actuators?
while(!stopped){
rt_sem_p(&readDone,0);
int c;
for(c=0;c<nStep;c++){
if(stepStatus[c])
step[c](sensors); // or equivalent
}
rt_sem_v(&executionDone);
rt_sem_p(&writeDone,0);
for(c=0;c<nStep;c++){
if(stepStatus[c])
condition[c](sensor,actuators);
}
rt_task_wait_period(NULL);
}
return;
}
void demo (void *arg){
sleep(1);
RT_TASK *curtask;
RT_TASK_INFO curtaskinfo;
//inquire current task
curtask = rt_task_self();
int retval;
retval = rt_task_inquire(curtask,&curtaskinfo);
if(retval<0){
rt_printf("inquiring error %d:%s\n",-retval,strerror(-retval));
}
//print task name
retval = * (int *)arg;
RTIME p = 1e9;
p*=retval;
rt_task_set_periodic(NULL,TM_NOW,p);
while(1){
rt_printf("[%s] %d s periodic\n", curtaskinfo.name,retval);
rt_task_wait_period(NULL);
}
}
int _rtapi_task_start_hook(task_data *task, int task_id,
unsigned long int period_nsec) {
int retval;
if ((retval = rt_task_set_periodic(ostask_array[task_id], TM_NOW,
period_nsec)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: rt_task_set_periodic() task_id %d "
"periodns=%ld returns %d\n",
task_id, period_nsec, retval);
return -EINVAL;
}
if ((retval = rt_task_start(ostask_array[task_id], task->taskcode,
(void*)task->arg )) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: rt_task_start() task_id %d returns %d\n",
task_id, retval);
return -EINVAL;
}
return 0;
}
void publisher_proc(void *arg)
{
RTROSPublisher* pObj = (RTROSPublisher*)arg;
int i,j;
rt_task_set_periodic(NULL, TM_NOW, 50e5); // 1e6 -> 1ms 5e5 -> 500us
while (1)
{
rt_task_wait_period(NULL); //wait for next cycle
// Data set
for(i=0;i<4;i++)
{
dxlDevice[i].mutex_acquire();
}
int _cnt = 0;
for(i=0;i<4;i++)
{
for(j=0;j<nDXLCount[i];j++)
{
// SI
pObj->jointMsg->angle[_cnt] = dxlDevice[i][j].position_rad();
pObj->jointMsg->velocity[_cnt] = dxlDevice[i][j].velocity_radsec();
pObj->jointMsg->current[_cnt] = dxlDevice[i][j].current_amp();
pObj->jointMsg->updated[_cnt] = dxlDevice[i][j].updated;
_cnt++;
}
}
for(i=0;i<4;i++)
{
dxlDevice[i].mutex_release();
}
pObj->pubState.publish(pObj->jointMsg);
}
}
void watchdog(void *arg) {
//sem ??
rt_printf("twatchdog : Debut de l'éxecution de periodique à 1s\n");
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
int cpt=0;
int status=1;
while (1) {
rt_task_wait_period(NULL);
rt_printf("twatchdog : Activation périodique\n");
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
status = etatCommMoniteur;
rt_mutex_release(&mutexEtat);
if (status == STATUS_OK) {
cpt++;
rt_mutex_acquire(&mutexRobot, TM_INFINITE);
status=d_robot_reload_wdt(robot);
rt_mutex_release(&mutexRobot);
if (status == STATUS_OK) {
cpt--;
}
if (cpt>=7){
Verif_Comm(cpt);
}
}
}
}
/* NOTE: error handling omitted. */
void demo(void *arg)
{
RTIME now, previous;
/*
* Arguments: &task (NULL=self),
* start time,
* period (here: 1 s)
*/
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
previous = rt_timer_read();
while (1) {
rt_task_wait_period(NULL);
now = rt_timer_read();
/*
* NOTE: printf may have unexpected impact on the timing of
* your program. It is used here in the critical loop
* only for demonstration purposes.
*/
printf("Time since last turn: %ld.%06ld ms\n",
(long)(now - previous) / 1000000,
(long)(now - previous) % 1000000);
previous = now;
}
}
void ElevatorController::updateStatus() {
rt_task_set_periodic(NULL, TM_NOW, 75000000);
while(true)
{
rt_task_wait_period(NULL);
rt_mutex_acquire(&(this->rtData.mutex), TM_INFINITE);
int tempFloor = this->eStat.getCurrentFloor();
this->eStat.setCurrentFloor(this->getSimulator()->getCurrentFloor());
if(tempFloor != this->eStat.getCurrentFloor())
{
this->notifyFloorReached(this->eStat.getCurrentFloor());
}
this->eStat.setTaskAssigned(this->getSimulator()->getIsTaskActive());
this->eStat.setCurrentSpeed(this->getSimulator()->getCurrentSpeed());
this->eStat.setDirection(this->getSimulator()->getDirection());
this->eStat.setCurrentPosition((unsigned char)ceil(this->getSimulator()->geCurrentPosition()));
this->eStat.setTaskActive(this->eStat.getTaskAssigned());
int upHeapSize = this->getUpHeap().getSize();
int downHeapSize = this->getDownHeap().getSize();
if(upHeapSize==0 && downHeapSize==0){this->eStat.setGDFailedEmptyHeap(true);}
if(upHeapSize > 0)
{
int topItem = (int)(this->getUpHeap().peek());
if(this->eStat.getCurrentFloor() == topItem && !this->eStat.getTaskAssigned())
{
rt_printf("ST%d Task Completed %d\n", this->getID(), this->eStat.getDestination());
this->getUpHeap().pop();
}
}
if(downHeapSize > 0)
{
int topItem = (int)(this->getDownHeap().peek());
if(this->eStat.getCurrentFloor() == topItem && !this->eStat.getTaskAssigned())
{
rt_printf("ST%d Task Completed %d\n", this->getID(), this->eStat.getDestination());
this->getDownHeap().pop();
}
}
upHeapSize = this->getUpHeap().getSize();
downHeapSize = this->getDownHeap().getSize();
int total = upHeapSize + downHeapSize;
if(this->eStat.getDirection()==DIRECTION_UP && upHeapSize == 0 && downHeapSize > 0){ this->eStat.setServiceDirection(DIRECTION_DOWN); }
else if(this->eStat.getDirection()==DIRECTION_UP && upHeapSize > 0){ this->eStat.setServiceDirection(DIRECTION_UP); }
else if(this->eStat.getDirection()==DIRECTION_DOWN && downHeapSize == 0 && upHeapSize > 0){ this->eStat.setServiceDirection(DIRECTION_UP); }
else if(this->eStat.getDirection()==DIRECTION_DOWN && downHeapSize > 0){ this->eStat.setServiceDirection(DIRECTION_DOWN); }
if(total==0){this->updateMissedFloor(this->eStat.getDirection());}
this->releaseFreeCond();
rt_mutex_release(&(this->rtData.mutex));
this->updateStatusBuffer();
/*rt_printf("UPDATED : currentSpeed %d currentFloor %d task %d\n",
(unsigned int)this->eStat.getCurrentSpeed(), (unsigned int)this->eStat.getCurrentFloor(), this->eStat.getTaskActive());*/
}
}
void latency(void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks, fault_threshold;
RT_TIMER_INFO timer_info;
unsigned old_relaxed = 0;
err = rt_timer_inquire(&timer_info);
if (err) {
fprintf(stderr, "latency: rt_timer_inquire, code %d\n", err);
return;
}
fault_threshold = rt_timer_ns2tsc(CONFIG_XENO_DEFAULT_PERIOD);
nsamples = ONE_BILLION / period_ns / 1000;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err =
rt_task_set_periodic(NULL, start_ticks,
rt_timer_ns2ticks(period_ns));
if (err) {
fprintf(stderr, "latency: failed to set periodic, code %d\n",
err);
return;
}
for (;;) {
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt;
long overrun = 0;
long long sumj;
test_loops++;
for (count = sumj = 0; count < nsamples; count++) {
unsigned new_relaxed;
unsigned long ov;
expected_tsc += period_tsc;
err = rt_task_wait_period(&ov);
dt = (long)(rt_timer_tsc() - expected_tsc);
new_relaxed = sampling_relaxed;
if (dt > maxj) {
if (new_relaxed != old_relaxed
&& dt > fault_threshold)
max_relaxed +=
new_relaxed - old_relaxed;
maxj = dt;
}
old_relaxed = new_relaxed;
if (dt < minj)
minj = dt;
sumj += dt;
if (err) {
if (err != -ETIMEDOUT) {
fprintf(stderr,
"latency: wait period failed, code %d\n",
err);
exit(EXIT_FAILURE); /* Timer stopped. */
}
overrun += ov;
expected_tsc += period_tsc * ov;
}
if (freeze_max && (dt > gmaxjitter)
&& !(finished || warmup)) {
xntrace_user_freeze(rt_timer_tsc2ns(dt), 0);
gmaxjitter = dt;
}
if (!(finished || warmup) && need_histo())
add_histogram(histogram_avg, dt);
}
if (!warmup) {
if (!finished && need_histo()) {
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if (minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if (maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
}
if (warmup && test_loops == WARMUP_TIME) {
test_loops = 0;
warmup = 0;
}
}
}
void realtimeLoop(void *arg)
{
Q_UNUSED(arg);
int xenoRet = 0;
char bufChar[MAX_MESSAGE_LENGTH];
int bufInt[MAX_MESSAGE_LENGTH];
RealtimeController::Mode bufMode[MAX_MESSAGE_LENGTH];
RealtimeController::State bufState[MAX_MESSAGE_LENGTH];
int kp = 0, ki = 0, kd = 0;
int setValue = 0;
int actuatorVoltage = 0;
int actuatorVoltageToSend = 0;
int sensorVoltage = 0;
int sensorVoltageToSend = 0;
RealtimeController::Mode mode = RealtimeController::MODE_MANUAL;
RealtimeController::Mode modeToSend;
RealtimeController::State state = RealtimeController::STOPPED;
RealtimeController::State stateToSend;
int output = 0;
PiezoSensor* sensor;
PiezoActuator* actuator;
// varataan muisti anturille ja alustetaa se
sensor = new PiezoSensor();
sensor->init();
// varataan muisti toimilaitteelle ja alustetaan se
actuator = new PiezoActuator();
actuator->init();
// määritetään jaksonaika 500us eli 500000ns
RTIME interval = 500000;
// määritetään taskia kutsuttavaksi jaksonajan välein
xenoRet = rt_task_set_periodic(NULL, TM_NOW, interval);
while(1)
{
// ajastaa silmukan kulkemaan määritetyissä jaksonajoissa
if(rt_task_wait_period(NULL) < 0)
qDebug("task error in realtime loop");
if(state == RealtimeController::STARTED)
{
sensor->getValue(sensorVoltage);
sensorVoltage *= -1; // kytkentä laboratoriolaitteistolla on väärinpäin
// tarkastetaan ajetaanko PID-säätöä vai suoraa ohjausta
if( mode == RealtimeController::MODE_MANUAL)
{
// suorassa jänniteohjauksessa laitetaan suoraan käyttäjän syöttämä skaalattu arvo
// ulostuloon
output = actuatorVoltage;
}
else
{
// PID-säädössä ajetaan algoritmi ja asetetaan ulostulo
output = pid(setValue/1000.0, sensorVoltage/1000.0, kp/1000.0, ki/1000.0, kd/1000.0);
actuatorVoltage = output;
}
}
// asetetaan ulostulo oikeasti toimilaitteelle
actuator->setValue(output);
// qDebug()<< "Set " << output<< "to actuator";
// luetaan käyttöliittymästä mahdollisesti tullut viesti
xenoRet = 0;
xenoRet = rt_pipe_read(&pipe_desc, bufChar, sizeof(bufChar), TM_NONBLOCK);
// jos oltiin saatu viesti, luetaan sen sisältö ja muutetaan tai
// palautetaan lukuarvoja ja tiloja tarvittaessa
// mutexia ei tarvita, koska tämä tehdään joka kierroksella
// säätölaskennan ja säädön jälkeen
if(xenoRet > 0)
{
if(!strcmp(bufChar, "reaSen"))
{
sensorVoltageToSend = sensorVoltage;
int *pBuf = &sensorVoltageToSend;
if(rt_pipe_write(&pipe_desc, (void*)pBuf, sizeof(bufInt), P_NORMAL) < 1)
qDebug("rt write error");
bufChar[0] = '0';
}
else if(!strcmp(bufChar, "reaOut"))
{
actuatorVoltageToSend = actuatorVoltage;
int *pBuf = &actuatorVoltageToSend;
if(rt_pipe_write(&pipe_desc, (void*)pBuf, sizeof(bufInt), P_NORMAL) < 1)
qDebug("rt write error");
bufChar[0] = '0';
}
else if(!strcmp(bufChar, "reaMod"))
{
modeToSend = mode;
RealtimeController::Mode *pBuf = &modeToSend;
if(rt_pipe_write(&pipe_desc, (void*)pBuf, sizeof(bufInt), P_NORMAL) < 1)
qDebug("rt write error");
bufChar[0] = '0';
}
else if(!strcmp(bufChar, "reaSta"))
{
stateToSend = state;
RealtimeController::State *pBuf = &stateToSend;
if(rt_pipe_write(&pipe_desc, (void*)pBuf, sizeof(bufInt), P_NORMAL) < 1)
qDebug("rt write error");
bufChar[0] = '0';
}
else if (!strcmp(bufChar, "setSta"))
{
if(rt_pipe_read(&pipe_desc, bufState, sizeof(bufState), TM_INFINITE) < 1)
qDebug("rt read error");
state = *(RealtimeController::State*)bufState;
bufChar[0] = '0';
qDebug("Set state: %d", state);
}
else if (!strcmp(bufChar, "setPID"))
{
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
kp = *(int*)bufInt;
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
ki = *(int*)bufInt;
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
kd = *(int*)bufInt;
bufChar[0] = '0';
qDebug("Set PID-parameters p: %d, i: %d, d: %d", kp,ki,kd);
}
else if (!strcmp(bufChar, "setSet"))
{
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
setValue = *(int*)bufInt;
bufChar[0] = '0';
qDebug("Set setValue: %d", setValue);
}
else if (!strcmp(bufChar, "setOut"))
{
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
actuatorVoltage = *(int*)bufInt;
bufChar[0] = '0';
qDebug("Set actuatorVoltage: %d", actuatorVoltage);
}
else if (!strcmp(bufChar, "setSca"))
{
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
sensor->setScale(*(int*)bufInt);
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
actuator->setScale(*(int*)bufInt);
bufChar[0] = '0';
qDebug("Set sensor and, actuator scales");
}
else if (!strcmp(bufChar, "setMod"))
{
if(rt_pipe_read(&pipe_desc, bufMode, sizeof(bufMode), TM_INFINITE) < 1)
qDebug("rt read error");
mode = *(RealtimeController::Mode*)bufMode;
bufChar[0] = '0';
qDebug("Set mode to: %d", mode);
}
else if(!strcmp(bufChar, "theEnd"))
{
qDebug("Bye!");
break;
}
else
qDebug("rt read error: unknown string");
}
}
}
int init_module(void)
{
int ret;
struct sockaddr_ll local_addr;
/* set destination address */
memset(&dest_addr, 0, sizeof(struct sockaddr_ll));
dest_addr.sll_family = AF_PACKET;
dest_addr.sll_protocol = htons(PROTOCOL);
dest_addr.sll_ifindex = local_if;
dest_addr.sll_halen = 6;
rt_eth_aton(dest_addr.sll_addr, dest_mac_s);
printk("destination mac address: %02X:%02X:%02X:%02X:%02X:%02X\n",
dest_addr.sll_addr[0], dest_addr.sll_addr[1],
dest_addr.sll_addr[2], dest_addr.sll_addr[3],
dest_addr.sll_addr[4], dest_addr.sll_addr[5]);
printk("local interface: %d\n", local_if);
/* create rt-socket */
sock = rt_dev_socket(AF_PACKET, SOCK_DGRAM, htons(PROTOCOL));
if (sock < 0) {
printk(" rt_dev_socket() = %d!\n", sock);
return sock;
}
/* bind the rt-socket to a port */
memset(&local_addr, 0, sizeof(struct sockaddr_ll));
local_addr.sll_family = AF_PACKET;
local_addr.sll_protocol = htons(PROTOCOL);
local_addr.sll_ifindex = local_if;
ret = rt_dev_bind(sock, (struct sockaddr *)&local_addr,
sizeof(struct sockaddr_ll));
if (ret < 0) {
printk(" rt_dev_bind() = %d!\n", ret);
goto cleanup_sock;
}
/* You may have to start the system timer manually
* on older Xenomai versions (2.0.x):
* rt_timer_start(TM_ONESHOT); */
ret = rt_task_create(&rt_recv_task, "recv_task", 0, 9, 0);
if (ret != 0) {
printk(" rt_task_create(rt_recv_task) = %d!\n", ret);
goto cleanup_sock;
}
ret = rt_task_start(&rt_recv_task, recv_msg, NULL);
if (ret != 0) {
printk(" rt_task_start(rt_recv_task) = %d!\n", ret);
goto cleanup_recv_task;
}
ret = rt_task_create(&rt_xmit_task, "xmit_task", 0, 10, 0);
if (ret != 0) {
printk(" rt_task_create(rt_xmit_task) = %d!\n", ret);
goto cleanup_recv_task;
}
ret = rt_task_set_periodic(&rt_xmit_task, TM_INFINITE, CYCLE);
if (ret != 0) {
printk(" rt_task_set_periodic(rt_xmit_task) = %d!\n", ret);
goto cleanup_xmit_task;
}
ret = rt_task_start(&rt_xmit_task, send_msg, NULL);
if (ret != 0) {
printk(" rt_task_start(rt_xmit_task) = %d!\n", ret);
goto cleanup_xmit_task;
}
return 0;
cleanup_xmit_task:
rt_task_delete(&rt_xmit_task);
cleanup_recv_task:
rt_task_delete(&rt_recv_task);
cleanup_sock:
rt_dev_close(sock);
return ret;
}
void latency (void *cookie)
{
int err, count, nsamples;
RTIME expected, period;
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
nsamples = ONE_BILLION / sampling_period;
period = rt_timer_ns2ticks(sampling_period);
expected = rt_timer_tsc();
err = rt_task_set_periodic(NULL,TM_NOW,sampling_period);
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
unsigned long ov;
expected += period;
err = rt_task_wait_period(&ov);
if (err)
{
if (err != -ETIMEDOUT)
rt_task_delete(NULL); /* Timer stopped. */
overrun += ov;
}
dt = (long)(rt_timer_tsc() - expected);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!finished && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = rt_timer_ticks2ns(minj);
maxjitter = rt_timer_ticks2ns(maxj);
avgjitter = rt_timer_ticks2ns(sumj / nsamples);
rt_sem_v(&display_sem);
}
}
void motor_cmd_routine(void *m_arg)
{
int ret;
RT_TIMER_INFO timer_info;
long long task_period;
unsigned long overruns = 0;
int16_t req_current = 0;
int sync_ref_counter=0;
float cos_el;
float sin_el;
float v_req_az;
float V_REQ_AZ = 0;
float P_term_az, error_az;
float p_az = 20.0;
float i_az = 1.0;
static float az_integral = 0.0;
float I_term_az, INTEGRAL_CUTOFF=0.5;
printf("Starting Motor Commanding task\n");
rt_timer_inquire(&timer_info);
if (timer_info.period == TM_ONESHOT)
{
// When using an aperiodic timer, task period is specified in ns
task_period = rt_timer_ns2ticks(1000000000ll / 100);
}
else
{
// When using a periodic timer, task period is specified in number of timer periods
task_period = (1000000000ll / 100) / timer_info.period;
}
ret = rt_task_set_periodic(NULL, TM_NOW, task_period);
if (ret)
{
printf("error while set periodic, code %d\n", ret);
return;
}
// Make sure we are in primary mode before entering the timer loop
rt_task_set_mode(0, T_PRIMARY, NULL);
while (!stop)
{
unsigned long ov;
// Wait for next time period
ret = rt_task_wait_period(&ov);
if (ret && ret != -ETIMEDOUT)
{
printf("error while rt_task_wait_period, code %d (%s)\n", ret,
strerror(-ret));
break;
}
overruns = overruns + ov;
ecrt_master_receive(master);
ecrt_domain_process(domain);
// write application time to master
ecrt_master_application_time(master, rt_timer_tsc2ns(rt_timer_tsc()));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; // sync every cycle
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
/*******************************************************************\
* Card0: Drive the Azimuth Motor (Reaction Wheel) *
\*******************************************************************/
/* Read sin and cos of the inner frame elevation, calculated by mcp */
// cos_el = 1.0; //( COS_EL*0.000030517578125 ) - 1.0;
// sin_el = 0.0; //( SIN_EL*0.000030517578125 ) - 1.0;
//
// v_req_az = 0.0; //(float)(V_REQ_AZ-32768.0)*0.0016276041666666666666666666666667; // = vreq/614.4
//
// //roll, yaw contributions to az both -'ve (?)
// error_az = (gy_ifroll*sin_el + gy_ifyaw*cos_el) + v_req_az;
//
// P_term_az = p_az*error_az;
//
// if( (p_az == 0.0) || (i_az == 0.0) ) {
// az_integral = 0.0;
// } else {
// az_integral = (1.0 - INTEGRAL_CUTOFF)*az_integral + INTEGRAL_CUTOFF*error_az;
// }
//
// I_term_az = az_integral * p_az * i_az;
// if (I_term_az > 100.0) {
// I_term_az = 100.0;
// az_integral = az_integral *0.9;
// }
// if (I_term_az < -100.0) {
// I_term_az = -100.0;
// az_integral = az_integral * 0.9;
// }
// if (P_term_az > 1.0 || P_term_az < -1.0) printf("error_az: %f\tI: %f\tP: %f\n", error_az, I_term_az, P_term_az);
// req_current = 0.5 *(-(P_term_az + I_term_az) ) ;
req_current = 100;
if (req_current > 200)
printf("Error! Requested current is %d\n", req_current);
else {
EC_WRITE_S16(rx_controller_state.current_val, req_current);
}
ecrt_domain_queue(domain);
ecrt_master_send(master);
}
//switch to secondary mode
ret = rt_task_set_mode(T_PRIMARY, 0, NULL);
if (ret)
{
printf("error while rt_task_set_mode, code %d\n", ret);
return;
}
}
void* vmcThreadFunction(void* param)
#endif
{
#ifdef REAL_TIME
rt_task_set_periodic(NULL, TM_NOW, 40000000);
#endif
CVmc* pThread = (CVmc*)param; // typecast
ros::NodeHandle n;
ros::Publisher pub = n.advertise<volksbot::ticks>("VMC", 20);
volksbot::ticks t;
t.header.frame_id = "/base_link";
ros::Subscriber sub = n.subscribe("Vel", 100, Vcallback,
ros::TransportHints().reliable().udp().maxDatagramSize(100));
ros::Subscriber cmd_vel_sub_ = n.subscribe<geometry_msgs::Twist>("cmd_vel", 10, CVcallback,
ros::TransportHints().reliable().udp().maxDatagramSize(100));
// ros::TransportHints().reliable().tcp().tcpNoDelay(true));
ros::ServiceServer service = n.advertiseService("Controls", callback);
pThread->clearResponseList();
pThread->addStateToResponseList(CVmc::MOTOR_TICKS_ABSOLUTE);
ROS_INFO("VolksBot starting main loop!");
pThread->enterCriticalSection();
while(pThread->isConnected())
{
ros::Time current = ros::Time::now();
if (current - lastcommand < ros::Duration(50.5) ) {
#ifdef REAL_TIME
pThread->setMotors(leftvel, rightvel);
#else
pThread->_pwmOut2 = -1*leftvel*pThread->_maxRpm/100;
pThread->_pwmOut1 = rightvel*pThread->_maxRpm/100;
#endif
} else {
#ifdef REAL_TIME
pThread->setMotors(0, 0);
#else
pThread->_pwmOut2 = 0;
pThread->_pwmOut1 = 0;
#endif
}
// old setting motor values could crash because of wrong mutex use
// pThread->_apiObject->useVMC().MotorRPMs.Set(pThread->_pwmOut1,
// pThread->_pwmOut2, pThread->_pwmOut3);
CStorage store = pThread->_apiObject->useVMC();
store.MotorRPMs.Set(pThread->_pwmOut1,
pThread->_pwmOut2, pThread->_pwmOut3);
if(pThread->_digitalInputUpdate) {
pThread->_apiObject->useVMC().DigitalIN.Update();
}
// setup ticks message with timestamp, ticks and velocity commands
#ifdef REAL_TIME
uint64_t ts = store.getTimeStamp();
t.header.stamp.sec = ts / 1000000000;
t.header.stamp.nsec = ts % 1000000000;
#else
t.header.stamp = ros::Time::now();
#endif
t.left = -1* (int) ((long)store.Motor[1].AbsolutRotations.getValue());
t.right = (int) ((long)store.Motor[0].AbsolutRotations.getValue());
t.vx = vx;
t.vth = vth;
pub.publish(t);
#ifdef REAL_TIME
rt_task_wait_period(NULL);
#else
pThread->wait(pThread->_cycleTime);
#endif
}
pThread->leaveCriticalSection();
#ifndef REAL_TIME
return 0;
#endif
}
int RT::OS::setPeriod(RT::OS::Task task,long long period) {
xenomai_task_t *t = reinterpret_cast<xenomai_task_t *>(task);
t->period = period;
return rt_task_set_periodic(&t->task,TM_NOW,period);
}
/* alarm_handler
** is triggered once 10s is passed to inform player
** wait time has expired - issue start
*/
void alarm_handler(void *arg)
{
int sock = *(int *)&arg;
int echolen;
/* start the countdown */
rt_task_set_periodic(NULL, TM_NOW, 1000000000ULL);
int i;
for(i = 15; i >= 0; i--){
rt_task_wait_period(NULL);
rt_mutex_acquire(&txMutex[E_WAIT], TM_INFINITE);
memset(countData, 0, sizeof(countData));
sprintf(countData, "count;%d;", i);
padString(countData);
rt_mutex_release(&txMutex[E_WAIT]);
echolen = strlen(countData);
if(send(sock, countData,echolen, 0) != echolen)
rt_err("send()");
/*2 View mode case*/
if(viewMode > 1){
if(send(viewSocket2, countData, echolen, 0) != echolen)
rt_err("send()");
}
}
if(!issuedStart)
rt_task_delete(NULL); //not need two players connected
rt_mutex_acquire(&autoStart, TM_INFINITE);
issuedStart = true;
rt_mutex_release(&autoStart);
AIMode = true;
printf("FORCED START client playing with AI!\n");
/* below we emulate the add, start */
addPong();
echolen = strlen(pongData[0]);
if(send(sock, pongData[0],echolen, 0) != echolen)
rt_err("send()");
/*2 View mode case*/
if(viewMode > 1){
if(send(viewSocket2, pongData[0], echolen, 0) != echolen)
rt_err("send()");
}
echolen = strlen(pongData[1]);
if(send(sock, pongData[1],echolen, 0) != echolen)
rt_err("send()");
/*2 View mode case*/
if(viewMode > 1){
if(send(viewSocket2, pongData[1], echolen, 0) != echolen)
rt_err("send()");
}
start();
echolen = strlen(gameData);
if(send(sock, gameData, echolen, 0) != echolen)
rt_err("send()");
/*2 View mode case*/
if(viewMode > 1){
if(send(viewSocket2, gameData, echolen, 0) != echolen)
rt_err("send()");
}
echolen = strlen(ballData);
if(send(sock, ballData, echolen, 0) != echolen)
rt_err("send()");
/*2 View mode case*/
if(viewMode > 1){
if(send(viewSocket2, ballData, echolen, 0) != echolen)
rt_err("send()");
}
if(rt_task_resume(&recvThread[0]))
rt_err("rt_task_resume()");
if(rt_task_resume(&ballThread))
rt_err("rt_task_resume()");
}
/* Task objects cleanup */
void hit_task_cleanup_objects(){
if(hit_task_mutex_created){
hit_task_mutex_created = 0;
rt_mutex_delete(&hit_task_mutex);
}
}
void hit_task_init(){
int i;
for(i = 0; i < NB_WEAPONS; i++){
if(weapons[i].weapon_type == GUN)
weapons[i].timing_charge.now = weapons[i].timing_charge.max;
else
weapons[i].timing_charge.now = 0;
weapons[i].timing_charge.last = 0;
weapons[i].timing_charge.time_current = 0;
weapons[i].timing_led.now = 0;
}
for(i = 0; i < NB_MAX_BULLETS; i++){
bullets[i].weapon = NULL;
}
for(i = 0; i < NB_MAX_BOMBS; i++){
bombs[i].weapon = NULL;
}
}
//!*****************HIT TASK***********************/
void hit_task(void *cookie){
invader_t *invader;
bullet_t *bullet;
int i, j;
int16_t y;
uint8_t removed; //TODO remove this and try
uint8_t impact = 0; //!flag de collision pour la tache hit
(void)cookie;
//! On définit la période de la tache
rt_task_set_periodic(NULL, TM_NOW, 50*MS);
//! init
hit_task_init();
for (;;) {
rt_task_wait_period(NULL);
if(!game_break){
//! On verrouille les bullets
hit_lock();
//! On verrouille le vaisseau
ship_lock();
//! On verrouille les invaders
invaders_lock();
//pour chaque bullet on va tester les collisions
for (i=0;i<NB_MAX_BULLETS;i++){
removed = 0;
if(bullets[i].weapon != NULL){
impact = 0;
//!current object
bullet = &bullets[i];
// On déplace la bullet
bullet->hitbox.y -= bullet->weapon->speed;
y = bullet->hitbox.y;
//suppression des bullets en haut de l'écran
if(y <= 0){
if(bullet->weapon->weapon_type != RAIL){
// Gestion des points lors de la sortie d'un bullet
if(bullet->weapon->weapon_type != WAVE && game_points >= 1){
//game_points -= 1;
}
if(!removed){
remove_bullet(*bullet, i);
removed = 1;
}
// On met a jour les spef concernant la precision de tir
game_bullet_used++;
continue;
}
}
//bullet : hit test avec les invaders
for (j=0;j<wave.invaders_count;j++){
if(wave.invaders[j].hp > 0){
//!current object
invader = &wave.invaders[j];
//hit test
if(hit_test(invader->hitbox, bullet->hitbox) == 0){
impact = 1;
//applique les dégats à l'invader
if(invader->hp >= bullet->weapon->damage){
// On met a jour les points de vie de l'invader
invader->hp-= bullet->weapon->damage;
// On met a jour les points
game_points += 1;
}
else{
// On met a jour les points de vie de l'invader
invader->hp = 0;
// On met a jour les points
game_points += 10;
}
// On met a jour les spef concernant la precision de tir
game_bullet_used++;
game_bullet_kill++; // La bullet à touché sa cible
}//if hit test
}
}//pour chaque invader
//bullet : hit test avec les bombes
for(j=0;j<NB_MAX_BOMBS;j++){
if(bombs[j].weapon != NULL){
//hit test
if(hit_test(bombs[j].hitbox, bullet->hitbox) == 0){
impact = 1;
//détruit la bombe
remove_bullet(bombs[j], j);
}
}
}
//bullet : hit test avec les autres bullets
for(j=0;j<NB_MAX_BULLETS;j++){
if( (bullets[j].weapon != NULL) &&
(&bullets[j] != bullet) && //Si différent de la bullet actuelle
(bullets[j].weapon->weapon_type != RAIL) && //Si ce n'est pas le laser
(bullets[j].weapon->weapon_type != WAVE) //Si ce n'est pas l'onde de choc
){
//hit test
if(hit_test(bullets[j].hitbox, bullet->hitbox) == 0){
impact = 1;
//détruit la bullet
if(!removed){
remove_bullet(bullets[j], j);
removed = 1;
}
}
}
}
//si un impact a été detecté pour la bullet principale, on la supprime
if( impact &&
!(bullet->weapon->weapon_type == WAVE) &&
!(bullet->weapon->weapon_type == RAIL) ){
if(!removed){
remove_bullet(*bullet, i);
removed = 1;
}
}
}//if non null
}//pour chaque bullet pirncipale
//pour chaque bombe on va tester les collisions
for(i=0;i<NB_MAX_BOMBS;i++){
if(bombs[i].weapon != NULL){
// On déplace les bombes
bombs[i].hitbox.y += bombs[i].weapon->speed;
//hit test avec le vaisseau
if(hit_test(ship.hitbox, bombs[i].hitbox) == 0){
//applique les dommages au vaisseau et supprime la bombe
if(ship.hp > 0){
ship.hp--;
if(ship.hp==0){
game_over = 1;
}
}
remove_bullet(bombs[i], i);
}
}
}
//invaders : hit test avec le vaisseau
for (i=0;i<wave.invaders_count;i++){
if(wave.invaders[i].hp > 0 &&
hit_test(ship.hitbox, wave.invaders[i].hitbox) == 0){
ship.hp = 0;
game_over = 1;
}
}
//traitement spécial pour le rail (animation)
if(rail_id != -1 && rail_timeout <= 0){
remove_bullet(bullets[rail_id],rail_id);
rail_id = -1;
}else
rail_timeout--;
// On deverrouille les invaders
invaders_unlock();
// On deverrouille le vaisseau
ship_unlock();
// On deverrouille les bullets
hit_unlock();
}else{
}
}
}//hit_task
void lMotor(void *arg)
{
int fd, per;
char buf[MAX_BUF];
char duty_cycle[14];
char Period[14];
RTIME now, previous;
long MAX = 0;
int flag = 1;
//Set polarity
snprintf(buf, sizeof(buf), "/sys/devices/ocp.3/pwm_test_P9_22.13/polarity");
fd = open(buf, O_RDWR);
if(fd < 0){
perror("Problem opening Polarity file - left motor");
}
//polarity 0: 0 Duty - 0V
write(fd, "0", 5);
close(fd);
//Set period
snprintf(buf, sizeof(buf), "/sys/devices/ocp.3/pwm_test_P9_22.13/period");
per = 10000;
sprintf(Period,"%d",per);
fd = open(buf, O_RDWR);
if(fd < 0){
perror("Problem opening Period file - left motor");
}
write(fd, &Period, 10);
close(fd);
// set duty cycle in the periodc task
snprintf(buf, sizeof(buf), "/sys/devices/ocp.3/pwm_test_P9_22.13/duty");
rt_task_set_periodic(NULL, TM_NOW, period);
rt_printf("Running Left motor task!\n");
while (1){
rt_task_wait_period(NULL);
previous = rt_timer_read();
// set duty cycle
sprintf(duty_cycle,"%d",duty);
fd = open(buf, O_RDWR); //Open ADC as read only
if(fd < 0){
perror("Problem opening Duty file - left motor");
}
write(fd, &duty_cycle, 10); //read upto 4 digits 0-1799
close(fd);
if (flag){
MAX = now- previous;
flag = 0;
}
if((long)((now - previous)) > MAX){
MAX = (long)((now - previous)) ;
}
rt_printf("WCET left Motor: %ld \n", MAX);
}
return;
}
void envoyer_image(void *arg) {
DMessage *message = d_new_message();
DMessage *message_position = d_new_message();
DMessage *message_arena = d_new_message();
DImage *image = d_new_image();
DJpegimage *jpegimage = d_new_jpegimage();
char arena_found[] = "arena found";
rt_printf("tstreaming : Envoi de l'image à ~2.5img/s\n");
rt_task_set_periodic(NULL, TM_NOW, 400000000);
while (1) {
/* Attente de l'activation périodique */
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
camera->get_frame(camera, image);
switch (etat_camera) {
case 0 : /* streaming simple */
rt_mutex_release(&mutexEtatCamera);
break;
case 1 : /* streaming avec arena */
rt_mutex_release(&mutexEtatCamera);
/* détection de l'arène */
arena = (DArena *) image->compute_arena_position(image);
/*check si l'arene a été trouvée*/
if (arena != NULL) {
x_arena = arena->get_x(arena);
y_arena = arena->get_y(arena);
//rt_printf("x = %f; y = %f\n ", x_arena, y_arena);
/*envoie du message confirmant que l'arène a été trouvée*/
message_arena->set(message_arena, ACTION_ARENA_IS_FOUND, strlen(arena_found), arena_found);
serveur->send(serveur, message_arena);
d_imageshop_draw_arena(image, arena);
}
break;
case 2 : /* streaming avec position du robot */
rt_mutex_release(&mutexEtatCamera);
/* détection de la position du robot */
arena = (DArena *) image->compute_arena_position(image);
rt_mutex_acquire(&mutexPosition, TM_INFINITE);
position_robot = (DPosition *) image->compute_robot_position(image, arena);
if (position_robot != NULL) {
x_robot = position_robot->get_x(position_robot);
y_robot = position_robot->get_y(position_robot);
orientation_robot = position_robot->get_orientation(position_robot);
rt_mutex_release(&mutexPosition);
/* rt_printf("\n\n\nx = %f; y = %f, angle = %f\n\n\n ", x_robot, y_robot, orientation_robot); */
/*envoie de la position par message*/
message_position->put_position(message_position, position_robot);
serveur->send(serveur, message_position);
d_imageshop_draw_position(image, position_robot);
}
break;
case 3 : /* streaming avec position de l'arene et du robot */
rt_mutex_release(&mutexEtatCamera);
/* détection de la position du robot et de l'arene*/
arena = (DArena *) image->compute_arena_position(image);
rt_mutex_acquire(&mutexPosition, TM_INFINITE);
position_robot = (DPosition *) image->compute_robot_position(image, arena);
if (position_robot != NULL) {
x_robot = position_robot->get_x(position_robot);
y_robot = position_robot->get_y(position_robot);
/* rt_printf("\n\n\nx = %f; y = %f\n\n\n ", x_robot, y_robot); */
rt_mutex_release(&mutexPosition);
/*envoie de la position par message*/
message_position->put_position(message_position, position_robot);
serveur->send(serveur, message_position);
d_imageshop_draw_position(image, position_robot);
}
/*check si l'arene a été trouvée*/
if (arena != NULL) {
x_arena = arena->get_x(arena);
y_arena = arena->get_y(arena);
/* rt_printf("x = %f; y = %f\n ", x_arena, y_arena); */
/*envoie du message confirmant que l'arène a été trouvée*/
message_arena->set(message_arena, ACTION_ARENA_IS_FOUND, strlen(arena_found), arena_found);
serveur->send(serveur, message_arena);
d_imageshop_draw_arena(image, arena);
}
break;
default :
rt_printf("valeur de etat_camera corrompue\n");
}
/*compression et envoie de l'image*/
jpegimage->compress(jpegimage, image);
//jpegimage->print(jpegimage);
message->put_jpeg_image(message, jpegimage);
serveur->send(serveur, message);
}
}
void initStruct(void) {
int err;
/* Creation des mutex */
if (err = rt_mutex_create(&mutexEtat, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexMove, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexRobot, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexServer, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexArene, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexImage, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexPositionRobot, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexPositionVoulue, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutexValidArene, NULL)) {
rt_printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation des semaphores */
if (err = rt_sem_create(&semConnecterRobot, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semWatchdog, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semPosition, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semAcquArene, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semValidArene, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semBattery, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semWebcam, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&semMission, NULL, 0, S_FIFO)) {
rt_printf("Error semaphore create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation des taches */
if (err = rt_task_create(&tcommunicate, NULL, 0, PRIORITY_TCOMMUNICATE, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tconnect, NULL, 0, PRIORITY_TCONNECT, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tmove, NULL, 0, PRIORITY_TMOVE, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tsend, NULL, 0, PRIORITY_TSEND, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&twatchdog, NULL, 0, PRIORITY_TWATCHDOG, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tbattery, NULL, 0, PRIORITY_TBATTERY, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tcam, NULL, 0, PRIORITY_TCAM, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tposition, NULL, 0, PRIORITY_TPOSITION, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tarena, NULL, 0, PRIORITY_TARENA, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&tmission, NULL, 0, PRIORITY_TMISSION, 0)) {
rt_printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Definition des periodes */
rt_task_set_periodic(&tmove, TM_NOW, 200000000); // 200ms
rt_task_set_periodic(&tsend, TM_NOW, 200000000); // 200ms
rt_task_set_periodic(&twatchdog, TM_NOW, 1000000000); // 1s
rt_task_set_periodic(&tbattery, TM_NOW, 250000000); // 250ms
rt_task_set_periodic(&tcam, TM_NOW, 600000000); // 600ms
rt_task_set_periodic(&tposition, TM_NOW, 600000000); // 600ms
rt_task_set_periodic(&tmission, TM_NOW, 600000000); // 600ms
/* Creation des files de messages */
if (err = rt_queue_create(&queueMsgGUI, "toto", MSG_QUEUE_SIZE*sizeof(DMessage), MSG_QUEUE_SIZE, Q_FIFO))
{
rt_printf("Error msg queue create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation des structures globales du projet */
areneValidee = 1;
arena = 0;
robot = d_new_robot();
mvt = d_new_movement();
server = d_new_server();
image = d_new_image();
positionRobot = d_new_position();
positionVoulue = d_new_position();
etat_communication = malloc(sizeof(Etat_communication_t));
etat_communication->robot = 1;
etat_communication->moniteur = 1;
}
int init_module(void)
{
int ret;
unsigned int i;
struct sockaddr_in local_addr;
unsigned long dest_ip = rt_inet_aton(dest_ip_s);
if (size > 65505)
size = 65505;
printk("destination ip address %s=%08x\n", dest_ip_s,
(unsigned int)dest_ip);
printk("size %d\n", size);
/* fill output buffer with test pattern */
for (i = 0; i < sizeof(buffer_out); i++)
buffer_out[i] = i & 0xFF;
/* create rt-socket */
sock = rt_dev_socket(AF_INET,SOCK_DGRAM,0);
if (sock < 0) {
printk(" rt_dev_socket() = %d!\n", sock);
return sock;
}
/* extend the socket pool */
ret = rt_dev_ioctl(sock, RTNET_RTIOC_EXTPOOL, &add_rtskbs);
if (ret != (int)add_rtskbs) {
printk(" rt_dev_ioctl(RT_IOC_SO_EXTPOOL) = %d\n", ret);
goto cleanup_sock;
}
/* bind the rt-socket to a port */
memset(&local_addr, 0, sizeof(struct sockaddr_in));
local_addr.sin_family = AF_INET;
local_addr.sin_port = htons(PORT);
local_addr.sin_addr.s_addr = INADDR_ANY;
ret = rt_dev_bind(sock, (struct sockaddr *)&local_addr,
sizeof(struct sockaddr_in));
if (ret < 0) {
printk(" rt_dev_bind() = %d!\n", ret);
goto cleanup_sock;
}
/* set destination address */
memset(&dest_addr, 0, sizeof(struct sockaddr_in));
dest_addr.sin_family = AF_INET;
dest_addr.sin_port = htons(PORT);
dest_addr.sin_addr.s_addr = dest_ip;
/* You may have to start the system timer manually
* on older Xenomai versions (2.0.x):
* rt_timer_start(TM_ONESHOT); */
ret = rt_task_create(&rt_recv_task, "recv_task", 0, 9, 0);
if (ret != 0) {
printk(" rt_task_create(rt_recv_task) = %d!\n", ret);
goto cleanup_sock;
}
ret = rt_task_start(&rt_recv_task, recv_msg, NULL);
if (ret != 0) {
printk(" rt_task_start(rt_recv_task) = %d!\n", ret);
goto cleanup_recv_task;
}
ret = rt_task_create(&rt_xmit_task, "xmit_task", 0, 10, 0);
if (ret != 0) {
printk(" rt_task_create(rt_xmit_task) = %d!\n", ret);
goto cleanup_recv_task;
}
ret = rt_task_set_periodic(&rt_xmit_task, TM_INFINITE, CYCLE);
if (ret != 0) {
printk(" rt_task_set_periodic(rt_xmit_task) = %d!\n", ret);
goto cleanup_xmit_task;
}
ret = rt_task_start(&rt_xmit_task, send_msg, NULL);
if (ret != 0) {
printk(" rt_task_start(rt_xmit_task) = %d!\n", ret);
goto cleanup_xmit_task;
}
return 0;
cleanup_xmit_task:
rt_task_delete(&rt_xmit_task);
cleanup_recv_task:
rt_dev_close(sock);
rt_task_delete(&rt_recv_task);
return ret;
cleanup_sock:
rt_dev_close(sock);
return ret;
}
/**
* BORDERLINE THREAD
*/
void ImuInterfaceNonRT::communicationLoop()
{
/**
* Initialization
*/
#ifdef __XENO__
rt_task_shadow(NULL, "BorderlineImuCom", 0, 0);
// immediately go into secondary mode:
// rt_task_set_mode(T_PRIMARY,0,NULL);
#endif
bool stop_reading = stop_reading_;
try
{
imu_.openPort("/dev/ttyACM0");
}
catch(...)
{
return;
}
std::cout << "The port to the imu sensor has been opened." << std::endl;
std::cout << "Firmware Version Number: " << imu_.getFirmwareNumber() << std::endl;
double fix_off = 0;
try
{
imu_.initTime(fix_off);
}
catch(...)
{
return;
}
try
{
imu_.initGyros();
}
catch(...)
{
return;
}
uint64_t time;
uint32_t timestamp;
double accel[3];
double unstab_accel[3];
double angrate[3];
// double mag[3];
double orientation[9];
double stab_accel[/*3*/] = {0.0, 0.0, 0.0};
double dummy_angrate[3];
double stab_mag[3];
// double delta_angle[3];
// double delta_velocity[3];
SL_quat sl_orient;
SL_Cstate sl_pos, sl_pos_unstab;
try
{
// imu_.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE_MAG_ORIENT);
}
catch(...)
{
return;
}
std::cout << "Initial Timestamp: " << imu_.initTimestamp() << std::endl;
std::cout << "IMU: finished initialization, entering loop..." << std::endl;
// start_time_ = 1000000.0*std::clock()/CLOCKS_PER_SEC;
#ifdef __XENO__
rt_task_set_periodic(NULL, TM_NOW, 1000000*microstrain_3dmgx2_imu::IMU::DATA_RATE_DECIM);
#endif
while(!stop_reading)
{
// std::cout << "microseconds passed: " << 1000.0*std::clock()/CLOCKS_PER_SEC - start_time_ << std::endl;
// usleep(100000 - (int)(1000.0*std::clock()/CLOCKS_PER_SEC) % 100000);
for(int i=0; i<3; i++)
{
accel[i] = 0.0;
unstab_accel[i] = 0.0;
}
imu_.transactAccelAngrateOrientation(&time, unstab_accel, angrate, orientation, ×tamp);
imu_.transactStabAccAngrateStabMag(&time, stab_accel, dummy_angrate, stab_mag);
// imu_.transactDeltaAngleDeltaVel(&time, delta_angle, delta_velocity);
for(int i=0; i<3; i++)
{
accel[i] = unstab_accel[i] - stab_accel[i];
// delta_velocity[i] -= stab_accel[i] *0.001*microstrain_3dmgx2_imu::IMU::DATA_RATE_DECIM;
}
imuDataToSLQuat(accel, angrate, orientation, sl_orient, sl_pos, unstab_accel, sl_pos_unstab);
int reads = 0;
int writes = 0;
// const double leackage = 0.99;
/**
* PRIMARY MODE
*/
{
SauronsRing the_ring(mutex_);
for(unsigned int i=0; i<9; i++)
raw_rot_mat_[i] = orientation[i];
for(unsigned int i=0; i<3; i++)
{
raw_ang_vel_[i] = angrate[i];
raw_acc_[i] = accel[i];
raw_unstab_acc_[i] = unstab_accel[i];
// raw_delta_linvel_[i] *= leackage;
// raw_delta_linvel_[i] += delta_velocity[i];
// raw_delta_angle_[i] += delta_angle[i];
// sl_pos.xd[i+1] += leackage*position_.xd[i+1];
}
position_ = sl_pos;
position_unstab_ = sl_pos_unstab;
orientation_ = sl_orient;
timestamp_ = time;
stop_reading = stop_reading_;
num_writes_++;
reads = num_reads_;
writes = num_writes_;
initialized_ = true;
}
// std::cout << "left primary mode..." << std::endl;
#ifdef __XENO__
rt_task_wait_period(NULL);
#endif
};
// communication stopped
imu_.closePort();
std::cout << "leaving communication loop." << std::endl;
}
/**
* Tâche gérant les mouvements des projectiles et les impacts de
* ces derniers avec les vaisseaux
*/
void shots_impacts(void * cookie) {
int i, j;
// Configuration de la tâche périodique
if (TIMER_PERIODIC) {
err = rt_task_set_periodic(&shots_impacts_task, TM_NOW, PERIOD_TASK_SHOT);
if (err != 0) {
printk("Shots and impacts task set periodic failed: %d\n", err);
return;
}
} else {
err = rt_task_set_periodic(&shots_impacts_task, TM_NOW, PERIOD_TASK_SHOT * MS);
if (err != 0) {
printk("Shots and impacts task set periodic failed: %d\n", err);
return;
}
}
while (1) {
rt_task_wait_period(NULL);
/* Parcours la liste des tirs */
for(i=0; i<NB_MAX_SHOTS; i++) {
if(shot[i].enable == 1) {
/* Fait avancer/reculer le tir s'il est actif */
shot[i].y += shot[i].direction;
// Désactive le missile si celui-ci touche le bas de l ecran
// TODO : doit etre desactive lorsque celui-ci touche un joueur
if (shot[i].y >= EDGE_SOUTH - MISSILE_SIZE)
shot[i].enable = 0;
else if(shot[i].y <= EDGE_NORTH + MISSILE_SIZE)
shot[i].enable = 0;
// Si le shot est à la hauteur du joueur
if((shot[i].y > (LCD_MAX_Y-20)) && (shot[i].direction == DIRECTION_DOWN))
{
for(j = 0; j < 3; j++)
{
if(player[j].enable == 1)
{
// S'il y a une collision avec le joueur
if((shot[i].x > player[j].x) && (shot[i].x < (player[j].x + 16)))
{
//printk("Player touched\n");
player[j].enable++;
shot[i].enable = 0;
}
}
}
}
// Si le shot est dans la zone ennemis
else if((shot[i].y <= (LCD_MAX_Y-20)) && (shot[i].direction == DIRECTION_UP))
{
for(j = 0; j < nbEnnemis; j++)
{
if(ennemi[j].enable == 1)
{
// S'il y a une collision avec un ennemi
if( (shot[i].x > ennemi[j].x) && (shot[i].x < (ennemi[j].x + 16))
&& (shot[i].y > ennemi[j].y) && (shot[i].y < (ennemi[j].y + 16)) )
{
//printk("Ennemi n°%d touched\n", i);
ennemi[j].pv--;
rt_mutex_lock(&mutex_score, TM_INFINITE);
score += speed*10;
rt_mutex_unlock(&mutex_score);
if(ennemi[j].pv == 0)
{
ennemi[j].enable++;
}
shot[i].enable = 0;
}
}
}
}
}
}
}
}
void deplacer(void *arg) {
int status = 1;
int gauche;
int droite;
int compteur;
DMessage *message;
rt_printf("tconnect : Attente du sémarphore semConnectedRobot\n");
rt_sem_p(&semConnectedRobot, TM_INFINITE);
rt_printf("tmove : Debut de l'éxecution de periodique à 1s\n");
rt_task_set_periodic(NULL, TM_NOW, 200000000);
while (1) {
/* Attente de l'activation périodique */
rt_task_wait_period(NULL);
rt_printf("tmove : Activation périodique\n");
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
status = etatCommRobot;
rt_mutex_release(&mutexEtat);
if (status == STATUS_OK) {
rt_mutex_acquire(&mutexMove, TM_INFINITE);
switch (move->get_direction(move)) {
case DIRECTION_FORWARD:
gauche = MOTEUR_ARRIERE_LENT;
droite = MOTEUR_ARRIERE_LENT;
break;
case DIRECTION_LEFT:
gauche = MOTEUR_ARRIERE_LENT;
droite = MOTEUR_AVANT_LENT;
break;
case DIRECTION_RIGHT:
gauche = MOTEUR_AVANT_LENT;
droite = MOTEUR_ARRIERE_LENT;
break;
case DIRECTION_STOP:
gauche = MOTEUR_STOP;
droite = MOTEUR_STOP;
break;
case DIRECTION_STRAIGHT:
gauche = MOTEUR_AVANT_LENT;
droite = MOTEUR_AVANT_LENT;
break;
}
rt_mutex_release(&mutexMove);
status = robot->set_motors(robot, gauche, droite);
Verif_Comm(status);
}
/*if (status != STATUS_OK) {
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommRobot = status;
rt_mutex_release(&mutexEtat);
rt_mutex_acquire(&mutexCpt, TM_INFINITE);
compteur = cpt_perte_com;
rt_mutex_release(&mutexCpt);
if (compteur == 6) { // si on vraiment perdu la connection
rt_mutex_acquire(&mutexCpt, TM_INFINITE);
cpt_perte_com = 0 ;
rt_mutex_release(&mutexCpt);
message = d_new_message();
message->put_state(message, status);
// On remet à l'état initial
// On envoie le message au moniteur
rt_printf("tmove : Envoi message\n");
if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
message->free(message);
}
}
else { // sinon on refait le test
rt_mutex_acquire(&mutexCpt, TM_INFINITE);
cpt_perte_com ++ ;
rt_mutex_release(&mutexCpt);
} */
}
}
void latency (void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks;
RT_TIMER_INFO timer_info;
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
err = rt_timer_inquire(&timer_info);
if (err)
{
fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err);
return;
}
nsamples = ONE_BILLION / period_ns;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err = rt_task_set_periodic(NULL,start_ticks,rt_timer_ns2ticks(period_ns));
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
long overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
expected_tsc += period_tsc;
err = rt_task_wait_period(NULL);
if (err)
{
if (err != -ETIMEDOUT)
{
fprintf(stderr,"latency: wait period failed, code %d\n",err);
rt_task_delete(NULL); /* Timer stopped. */
}
overrun++;
}
dt = (long)(rt_timer_tsc() - expected_tsc);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!(finished || warmup) && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if(!warmup)
{
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if(minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if(maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
}
if(warmup && test_loops == WARMUP_TIME)
{
test_loops = 0;
warmup = 0;
}
}
}
void latency (void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks;
RT_TIMER_INFO timer_info;
RT_QUEUE q;
rt_queue_create(&q, "queue", 0, 100, 0);
if (!(hard_timer_running = rt_is_hard_timer_running())) {
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
}
err = rt_timer_inquire(&timer_info);
if (err)
{
fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err);
return;
}
nsamples = ONE_BILLION / period_ns / 1;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err = rt_task_set_periodic(NULL,start_ticks,period_ns);
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
long overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
expected_tsc += period_tsc;
err = rt_task_wait_period(NULL);
if (err)
{
if (err != -ETIMEDOUT) {
rt_queue_delete(&q);
rt_task_delete(NULL); /* Timer stopped. */
}
overrun++;
}
dt = (long)(rt_timer_tsc() - expected_tsc);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!(finished || warmup) && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if(!warmup)
{
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if(minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if(maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
struct smpl_t { long minjitter, avgjitter, maxjitter, overrun; } *smpl;
smpl = rt_queue_alloc(&q, sizeof(struct smpl_t));
#if 1
smpl->minjitter = rt_timer_tsc2ns(minj);
smpl->maxjitter = rt_timer_tsc2ns(maxj);
smpl->avgjitter = rt_timer_tsc2ns(sumj / nsamples);
smpl->overrun = goverrun;
rt_queue_send(&q, smpl, sizeof(struct smpl_t), TM_NONBLOCK);
#endif
}
if(warmup && test_loops == WARMUP_TIME)
{
test_loops = 0;
warmup = 0;
}
}
}
void move_player(void * cookie) {
int i;
int err;
int border = 15;
int EdgeX_left = border;
int EdgeX_right = LCD_MAX_X - border;
int touch = 0;
struct ts_sample touch_info;
int speed = 5;
int maxLeft, maxRight;
// Configuration de la tâche périodique
if (TIMER_PERIODIC) {
err = rt_task_set_periodic(&move_task, TM_NOW, PERIOD_TASK_MOVE);
if (err != 0) {
printk("Move task set periodic failed: %d\n", err);
return;
}
} else {
err = rt_task_set_periodic(&move_task, TM_NOW, PERIOD_TASK_MOVE * MS);
if (err != 0) {
printk("Move task set periodic failed: %d\n", err);
return;
}
}
while (1) {
// Attend que l'utilisateur touche l'écran
while (touch == 0) {
rt_task_wait_period(NULL);
if (xeno_ts_read(&touch_info, 1, O_NONBLOCK) > 0) {
//printk("x = %d, y = %d\n", touch_info.x, touch_info.y);
touch = 1;
maxLeft = maxRight = 0;
/* Détecte quels vaisseaux sont au extrêmités */
for(i=0; i<NB_PLAYER; i++) {
if(player[i].enable) {
if(i%2)
maxRight = i;
else
maxLeft = i;
}
}
/* Déplacment solidaire des vaisseaux */
/* Droite */
if (touch_info.x > player[0].x) {
if (player[maxRight].x + 16 < EdgeX_right) {
for(i=0; i<NB_PLAYER; i++)
player[i].x += speed;
}
/* Gauche */
} else if(touch_info.x < player[0].x) {
if (player[maxLeft].x > EdgeX_left) {
for(i=0; i<NB_PLAYER; i++)
player[i].x -= speed;
}
}
while (xeno_ts_read(&touch_info, 1, O_NONBLOCK) > 0);
}
}
//printk("x_player = %d \n", player[0].x);
rt_task_wait_period(NULL);
touch = 0;
}
}
