int string_reader_init(t_string_reader *reader, t_source_file *file) { reader->file = file; position_init(&reader->position, file); return (0); }
void init_robot(struct robot *bot){ rs_init(&bot->rs); cs_init(&bot->cs_a); cs_init(&bot->cs_d); quadramp_init(&bot->qr_a); quadramp_init(&bot->qr_d); pid_init(&bot->pid_a); pid_init(&bot->pid_d); position_init(&bot->posr); //nous // bot->EVENT_DO_CS =1; bot->events = EVENT_DO_CS | EVENT_DO_POS; quadramp_init(&bot->qr_a); quadramp_init(&bot->qr_d); #ifdef UART_VERBOSE UART_CPutString("\r\n init robot structures : [OK]"); #endif }
Tour tour_copier(Tour tour) { Tour copie; copie.pion = tour.pion; copie.depart = position_init(tour.depart.x, tour.depart.y); copie.pile_coups = pileCoups_copier(&tour.pile_coups); return copie; }
Tour tour_init(Pion* p) { // Création du tour Tour tour; // On alloue ce qui peut-être alloué tour.pion = p; tour.depart = position_init(p->position.x, p->position.y); tour.pile_coups=pileCoups_init(); // on renvoie le tour initialisé return tour; }
////////////////////////////////////// // Main ///////////////////////////////////// int main(void) { //int a = 0; ///////////////////////////////////// ///TODO////////////////////////////// ///////////////////////////////////// // //participer a la coupe ///////////////////////////////////// ///////////////////////////////////// //Initialisations//////////////////// ///////////////////////////////////// uart_init(); time_init(128); printf("fdevopen \n"); fdevopen(uart0_send,uart0_recv, 1); // Affichage uart: tip -s 9600 -l /dev/ttyS0 // prog usb : // avarice -j /dev/ttyUSB0 --erase --program -f ./main.hex adc_init(); DDRE |= 0x00; PORTE |= 0x10;//active le pull up //initialisation du fpga sbi(XMCRA,SRW11); sbi(XMCRA,SRW00); sbi(MCUCR,SRE); wait_ms(1000); //on reset le fpga U_RESET = 0x42; sbi(PORTB,0); wait_ms(100); //on relache le reset cbi(PORTB,0); sbi(PORTE,3); wait_ms(100); //printf("stack begin :%d \n", &a); //init des interruptions printf("init i2c \n"); scheduler_init(); position_init(&pos); asserv_init(&asserv,&pos); trajectory_init(&traj,&pos,&asserv); i2cm_init(); //autorise les interruptions sei(); //infinite loop!!! //get_enc_value(); //init communication #ifdef ENABLE_MECA printf("init com \n"); initCom(); printf("end init com \n"); #endif //init A* initObstacle(); //init méca #ifdef ENABLE_MECA mecaCom(TIROIR_FERMER); mecaCom(PEIGNE_FERMER); #endif team = getTeam(); while(MAXIME);//boucle anti_maxime // while(1) // { // printf("gp2r = %d ,gp2l = %d ,gp2m = %d \n",adc_get_value(ADC_REF_AVCC | MUX_ADC0),adc_get_value(ADC_REF_AVCC | MUX_ADC1),adc_get_value(ADC_REF_AVCC | MUX_ADC2)); // } //avoidance_init(); //antipatinage_init(); // homologation(); // while(1); // test_evitement(); // while(1); if(team == RED) { disableSpinning(); findPosition(team); enableSpinning(); trajectory_goto_d(&traj, END, 20); trajectory_goto_a(&traj, END, 55); while(!trajectory_is_ended(&traj)); // // trajectory_goto_a(&traj, END, 90); // // trajectory_goto_a(&traj, END, 0); // while(!trajectory_is_ended(&traj)); asserv_set_vitesse_normal(&asserv); initTaskManager(&tkm); //addTask(&tkm, &returnFire, LOW_PRIORITY, R1); addTask(&tkm, &throwSpears, LOW_PRIORITY, RED); addTask(&tkm, &putPaint, HIGH_PRIORITY, team); // addTask(&tkm, &returnFire, LOW_PRIORITY, R3); // addTask(&tkm, &returnFire, HIGH_PRIORITY, R2); addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team); // addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team); // addTask(&tkm, &findPosition, HIGH_PRIORITY, REDPAINT); // addTask(&tkm, &returnFire, HIGH_PRIORITY, Y1); //addTask(&tkm, &throwSpears, HIGH_PRIORITY, YELLOW); addTask(&tkm, &putFruit, HIGH_PRIORITY, team); // addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3); // addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team); // addTask(&tkm, &putFruit, HIGH_PRIORITY, team); // addTask(&tkm, &returnFire, HIGH_PRIORITY, Y2); while (TIRETTE); printf("begin match \n"); avoidance_init(); while(doNextTask(&tkm)); printf("finish \n"); } else // Team jaune { disableSpinning(); findPosition(team); enableSpinning(); trajectory_goto_d(&traj, END, 20); trajectory_goto_a(&traj, END, -55); while(!trajectory_is_ended(&traj)); // addTask(&tkm, &returnFire, LOW_PRIORITY, R1); addTask(&tkm, &returnFire, LOW_PRIORITY, Y2); addTask(&tkm, &throwSpears, LOW_PRIORITY, YELLOW); addTask(&tkm, &putPaint, HIGH_PRIORITY, team); // addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3); // addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team); addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team); // addTask(&tkm, &returnFire, LOW_PRIORITY, R1); addTask(&tkm, &putFruit, HIGH_PRIORITY, team); // addTask(&tkm, &findPosition, HIGH_PRIORITY, YELLOWPAINT); //addTask(&tkm, &returnFire, HIGH_PRIORITY, R1); //addTask(&tkm, &throwSpears, HIGH_PRIORITY, RED); // addTask(&tkm, &returnFire, HIGH_PRIORITY, R3); // addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team); // addTask(&tkm, &putFruit, HIGH_PRIORITY, team); // addTask(&tkm, &returnFire, HIGH_PRIORITY, R2); while (TIRETTE); printf("begin match \n"); avoidance_init(); while(doNextTask(&tkm)); printf("finish \n"); // // attend que la tirette soit retirée // printf("begin match \n"); // printf("start pos: x %lf pos y %lf \n",position_get_x_cm(&pos),position_get_y_cm(&pos)); } printf("time: %ld \n",time_get_s()); while(1);//attention boucle infinie }
////////////////////////////////////// // Main ///////////////////////////////////// int main(void) { ///////////////////////////////////// //Initialisations//////////////////// ///////////////////////////////////// uart_init(); time_init(128); printf("fdevopen \n"); fdevopen(uart0_send,uart0_recv, 1); printf("test uart \n"); adc_init(); DDRE |= 0x00; PORTE |= 0x10; //active le pull up //initialisation du fpga sbi(XMCRA,SRW11); sbi(XMCRA,SRW00); sbi(MCUCR,SRE); wait_ms(1000); //on reset le fpga //U_RESET = 0x42; RESET_FPGA = 1 ; DDRB |= (1<<0); sbi(PORTB,0); wait_ms(100); //on relache le reset cbi(PORTB,0); sbi(PORTE,3); wait_ms(100); RESET = (1<<1) | (1<<4) |(1<<7); wait_ms(10); RESET = 0 ; RELATION = 15750; MOT_R=0; MOT_L=0; //init des interruptions printf("init i2c \n"); scheduler_init(); position_init(&pos); asserv_init(&asserv,&pos); trajectory_init(&traj,&pos,&asserv); //autorise les interruptions sei(); // while(1) // { // // printf("CLICKERS = %d \t %d \t %d \t \n",CLICKER_LEFT,CLICKER_MIDDLE,CLICKER_RIGHT); // printf("GP2 = %hu \t%hu \t%hu \t%hu\n", read_adc0(),read_adc1(),read_adc2(),read_adc3()); // } asserv_set_vitesse_fast(&asserv); find_position_from(GREENSTART); // SERVO1 = 1000; // SERVO2 = 1000; // SERVO3 = 1000; // SERVO4 = 1000; // SERVO5 = 1000; // SERVO6 = 1000; // SERVO7 = 1000; // SERVO8 = 1000; // SERVO9 = 1000; // SERVO10 = 1000; // SERVO11 = 1000; // SERVO12 = 1000; // SERVO13 = 1000; // SERVO14 = 1000; // SERVO15 = 1000; // SERVO16 = 1000; // while(1){ // // printf("PORTE=%d, PINE=%d \n", PORTE, PINE); // //printf("%hu\t%hu\t%hu\t%hu\n", read_adc0(),read_adc1(),read_adc2(),read_adc3()); // printf("%d=PINE & mask=%d %d %d %d \n", PINE, PINE &(1<<PE4),PINE &(1<<PE5), PINE &(1<<PE6), PINE &(1<<PE7)); // // printf("%d=PORTE & mask=%d %d %d %d \n \n \n", PORTE, PORTE &(1<<PE1),PORTE &(1<<PE2), PORTE &(1<<PE3), PORTE &(1<<PE4) ); // // printf("PINE >> pin=%d %d %d %d \n\n\n", PORTE >>PE1, PORTE >>PE2, PORTE >>PE3, PORTE >>PE4 ); // //printf("PINE >> PEx=%d %d %d %d \n", PINE & 0x10, PINE & 0x20, PINE & 0x30, PINE & 0x40); // wait_ms(1000); // } while(1); }
void microb_cs_init(void) { /* ROBOT_SYSTEM */ rs_init(&mainboard.rs); rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM); rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM); /* increase gain to decrease dist, increase left and it will turn more left */ rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value, LEFT_ENCODER, IMP_COEF * -1.0000); rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value, RIGHT_ENCODER, IMP_COEF * 1.0000); /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); /* POSITION MANAGER */ position_init(&mainboard.pos); position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM); position_set_related_robot_system(&mainboard.pos, &mainboard.rs); //position_set_centrifugal_coef(&mainboard.pos, 0.000016); position_use_ext(&mainboard.pos); /* TRAJECTORY MANAGER */ trajectory_init(&mainboard.traj); trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs, &mainboard.angle.cs); trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos); trajectory_set_speed(&mainboard.traj, 1500, 1500); /* d, a */ /* distance window, angle window, angle start */ trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.); /* ---- CS angle */ /* PID */ pid_init(&mainboard.angle.pid); pid_set_gains(&mainboard.angle.pid, 500, 10, 7000); pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095); pid_set_out_shift(&mainboard.angle.pid, 10); pid_set_derivate_filter(&mainboard.angle.pid, 4); /* QUADRAMP */ quadramp_init(&mainboard.angle.qr); quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */ quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */ /* CS */ cs_init(&mainboard.angle.cs); cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr); cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid); cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs); cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs); cs_set_consign(&mainboard.angle.cs, 0); /* Blocking detection */ bd_init(&mainboard.angle.bd); bd_set_speed_threshold(&mainboard.angle.bd, 80); bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50); /* ---- CS distance */ /* PID */ pid_init(&mainboard.distance.pid); pid_set_gains(&mainboard.distance.pid, 500, 10, 7000); pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095); pid_set_out_shift(&mainboard.distance.pid, 10); pid_set_derivate_filter(&mainboard.distance.pid, 6); /* QUADRAMP */ quadramp_init(&mainboard.distance.qr); quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */ quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */ /* CS */ cs_init(&mainboard.distance.cs); cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr); cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid); cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs); cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs); cs_set_consign(&mainboard.distance.cs, 0); /* Blocking detection */ bd_init(&mainboard.distance.bd); bd_set_speed_threshold(&mainboard.distance.bd, 60); bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); /* ---- CS fessor */ fessor_autopos(); /* PID */ pid_init(&mainboard.fessor.pid); pid_set_gains(&mainboard.fessor.pid, 300, 10, 150); pid_set_maximums(&mainboard.fessor.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.fessor.pid, 10); pid_set_derivate_filter(&mainboard.fessor.pid, 4); /* CS */ cs_init(&mainboard.fessor.cs); cs_set_correct_filter(&mainboard.fessor.cs, pid_do_filter, &mainboard.fessor.pid); cs_set_process_in(&mainboard.fessor.cs, fessor_set, NULL); cs_set_process_out(&mainboard.fessor.cs, encoders_microb_get_value, FESSOR_ENC); fessor_up(); /* ---- CS elevator */ elevator_autopos(); /* PID */ pid_init(&mainboard.elevator.pid); pid_set_gains(&mainboard.elevator.pid, 300, 10, 150); pid_set_maximums(&mainboard.elevator.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.elevator.pid, 10); pid_set_derivate_filter(&mainboard.elevator.pid, 4); /* CS */ cs_init(&mainboard.elevator.cs); cs_set_correct_filter(&mainboard.elevator.cs, pid_do_filter, &mainboard.elevator.pid); cs_set_process_in(&mainboard.elevator.cs, elevator_set, NULL); cs_set_process_out(&mainboard.elevator.cs, encoders_microb_get_value, ELEVATOR_ENC); elevator_down(); /* ---- CS wheel */ /* PID */ pid_init(&mainboard.wheel.pid); pid_set_gains(&mainboard.wheel.pid, 100, 100, 0); pid_set_maximums(&mainboard.wheel.pid, 0, 30000, 4095); pid_set_out_shift(&mainboard.wheel.pid, 5); pid_set_derivate_filter(&mainboard.wheel.pid, 4); /* CS */ cs_init(&mainboard.wheel.cs); cs_set_correct_filter(&mainboard.wheel.cs, pid_do_filter, &mainboard.wheel.pid); cs_set_process_in(&mainboard.wheel.cs, wheel_set, NULL); cs_set_process_out(&mainboard.wheel.cs, wheel_get_value, NULL); cs_set_consign(&mainboard.wheel.cs, 1000); /* set them on !! */ mainboard.angle.on = 0; mainboard.distance.on = 0; mainboard.fessor.on = 1; mainboard.elevator.on = 0; mainboard.wheel.on = 1; mainboard.flags |= DO_CS; scheduler_add_periodical_event_priority(do_cs, NULL, 5000L / SCHEDULER_UNIT, CS_PRIO); }
void init_posr(struct robot_position * _posr){ struct robot_position * posr = _posr; position_init(posr); }