static void main_task(void *arg)
{
RT_TASK *p;
int ret;
traceobj_enter(&trobj);
p = rt_task_self();
traceobj_assert(&trobj, p != NULL && rt_task_same(p, &t_main));
traceobj_mark(&trobj, 5);
ret = rt_alarm_start(&alrm, 200000000ULL, 200000000ULL);
traceobj_check(&trobj, ret, 0);
traceobj_mark(&trobj, 6);
ret = rt_task_suspend(&t_main);
traceobj_check(&trobj, ret, 0);
traceobj_mark(&trobj, 7);
ret = rt_alarm_delete(&alrm);
traceobj_check(&trobj, ret, 0);
traceobj_exit(&trobj);
}
int main(void)
{
int err;
int i;
RT_TASK task[NB_TACHES];
char nom_tache[80];
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
if ((err = rt_alarm_create(& alarme, "Ex01")) != 0) {
fprintf(stderr, "rt_alarm_create(): %s\n",
strerror(-err));
exit(EXIT_FAILURE);
}
for (i = 0; i < NB_TACHES-1; i++) {
snprintf(nom_tache, 80, "Alarme-%d\n", i+1);
if ((err = rt_task_spawn(& (task[i]), nom_tache, 0,
90-NB_TACHES + i, T_JOINABLE,
fonction_thread, (void *) (i+1))) != 0) {
fprintf(stderr, "rt_task_spawn: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
}
if ((err = rt_alarm_start(& alarme, TM_NOW, 1000000000)) != 0) {
fprintf(stderr, "rt_alarm_start: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
for (i = 0; i < NB_TACHES; i ++)
rt_task_join(& task[i]);
return 0;
}
void task_body (void *args) {
while (1) {
if(!rt_task_wait_period(NULL)) {
//write(servo1, &one, 1);
GPIO_SET = 1 << GPIO_PIN;
int t = (int)(1000000*(1.2+0.6*((++i/200)%3)/2.));
rt_alarm_start(&alarm_desc,t,TM_INFINITE);
}
if (!rt_alarm_wait(&alarm_desc)) {
//write(fichier, &zero, 1);
GPIO_CLR = 1 << GPIO_PIN;
}
}
}
void taskServo(void* cookie) {
OUT_SERVO_MOTOR* out=(OUT_SERVO_MOTOR*)cookie;
while (1) {
if(!rt_task_wait_period(NULL)) {
int time = ANGLE_TO_TIME(out->angle);
if(time > 0){
GPIO_SET = 1 << out->pin;
if(time < PERIODE_SERVO_MOTOR){
rt_alarm_start(&(out->alarm), time, TM_INFINITE);
if (!rt_alarm_wait(&(out->alarm))) {
GPIO_CLR = 1 << out->pin;
}
}
}
else {
GPIO_CLR = 1 << out->pin;
}
}
}
}
void taskMotor(void* cookie) {
OUT_MOTOR* out=(OUT_MOTOR*)cookie;
double coef=0;
double lastcoef=1;
int time=0;
double val;
getVitesse(out);//initialise les variables
while (1) {
if(!rt_task_wait_period(NULL)) {
lastcoef=coef;
val=getVitesse(out);
coef+=MOTOR_ASSERVISSEMENT*(out->vitesse- val);
if(coef>1)coef=1;
else if(coef<-1) coef=-1;
if(1 || lastcoef>=0 && coef<0 || lastcoef<=0 && coef>0 || coef == 0 && lastcoef !=0){
GPIO_CLR = (coef<=0) << out->pinIN1 | (coef>=0) << out->pinIN2;
GPIO_SET = (coef>0) << out->pinIN1 | (coef<0) << out->pinIN2;
}
time = COEFF_TO_TIME(coef);
if(time > 0){
GPIO_SET = 1 << out->pinPWM;
if(time < PERIODE_MOTOR){
rt_alarm_start(&(out->alarm), time, TM_INFINITE);
if (!rt_alarm_wait(&(out->alarm))) {
GPIO_CLR = 1 << out->pinPWM;
}
}
}
else {
GPIO_CLR = 1 << out->pinPWM;
}
}
}
}
