int colle(int x, int y) { int tab[6]; int tmp[2]; define_char(tab); tmp[0] = 0; while (tmp[0] < y) { tmp[1] = 0; while (tmp[1] < x) { print_line(tab, x, y, tmp); tmp[1]++; } ft_putchar('\n'); tmp[0]++; } return (0); }
/** * Scan the line */ void scan_line() { switch (sensor) { case 0b11111110: // Most left PV = -7; robot_moves_forward(); break; case 0b11111000: case 0b11111100: PV = -6; robot_moves_forward(); break; case 0b11111101: PV = -5; robot_moves_forward(); break; case 0b11110001: case 0b11111001: PV = -4; robot_moves_forward(); break; case 0b11111011: PV = -3; robot_moves_forward(); break; case 0b11100011: case 0b11110011: PV = -2; robot_moves_forward(); break; case 0b11110111: PV = -1; robot_moves_forward(); break; case 0b11100111: // Center PV = 0; robot_moves_forward(); break; case 0b11101111: PV = 1; robot_moves_forward(); break; case 0b11000111: case 0b11001111: PV = 2; robot_moves_forward(); break; case 0b11011111: PV = 3; robot_moves_forward(); break; case 0b10001111: case 0b10011111: PV = 4; robot_moves_forward(); break; case 0b10111111: PV = 5; robot_moves_forward(); break; case 0b00011111: case 0b00111111: PV = 6; robot_moves_forward(); break; case 0b01111111: // Most right PV = 7; robot_moves_forward(); break; case 0b11111111: // Loss if (PV < 0) { // PV = -8; lpwm = 150; rpwm = 185; robot_turns_left(); goto exit; } else if (PV > 0) { // PV = 8; lpwm = 180; rpwm = 155; robot_turns_right(); goto exit; } } error = SP - PV; P = (var_Kp * error) / 10; I = I + error; I = (I * var_Ki) / 10; rate = error - last_error; D = (rate * var_Kd) / 10; last_error = error; MV = P + I + D; if (MV == 0) { lpwm = MAXPWM - diffPWM; rpwm = MAXPWM; } else if (MV > 0) { // Moves left rpwm = MAXPWM - ((intervalPWM - 20) * MV); lpwm = (MAXPWM - (intervalPWM * MV) - 15) - diffPWM; if (lpwm < MINPWM) lpwm = MINPWM; if (lpwm > MAXPWM) lpwm = MAXPWM; if (rpwm < MINPWM) rpwm = MINPWM; if (rpwm > MAXPWM) rpwm = MAXPWM; } else if (MV < 0) { // Moves right lpwm = MAXPWM + ( ( intervalPWM - 20 ) * MV); rpwm = MAXPWM + ( ( intervalPWM * MV ) - 15 ); if (lpwm < MINPWM) lpwm = MINPWM; if (lpwm > MAXPWM) lpwm = MAXPWM; if (rpwm < MINPWM) rpwm = MINPWM; if (rpwm > MAXPWM) rpwm = MAXPWM; } exit: } void main(void) { // sensor PORTA=0x00; DDRA=0x00; //switch & sKi & sKa PORTB=0x0F; DDRB=0x00; //lcd PORTC=0x00; DDRC=0x00; //motor PORTD=0x00; DDRD=0xFF; // Timer/Counter 0 initialization TCCR0=0x00; TCNT0=0x00; OCR0=0x00; // Timer/Counter 1 initialization TCCR1A=0x00; TCCR1B=0x00; TCNT1H=0x00; TCNT1L=0x00; ICR1H=0x00; ICR1L=0x00; OCR1AH=0x00; OCR1AL=0x00; OCR1BH=0x00; OCR1BL=0x00; // Timer/Counter 2 initialization ASSR=0x00; TCCR2=0x00; TCNT2=0x00; OCR2=0x00; // External Interrupt(s) initialization MCUCR=0x00; MCUCSR=0x00; // Timer(s)/Counter(s) Interrupt(s) initialization TIMSK=0x01; // Analog Comparator initialization // Analog Comparator: Off // Analog Comparator Input Capture by Timer/Counter 1: Off ACSR=0x80; SFIOR=0x00; // LCD module initialization lcd_init(16); /* define user character 0 */ define_char(char0,0); // stop motor TCCR0=0x00; robot_stops(); show_menu_in_lcd(); TCCR0=0x05; #asm("sei") // read eeprom var_Kp = Kp; var_Ki = Ki; var_Kd = Kd; MAXPWM = (int)max_speed + 1; MINPWM = min_speed; intervalPWM = (max_speed - min_speed) / 8; PV = 0; error = 0; last_error = 0; robot_moves_forward(); while (1) { scan_line(); }; }
void main(void) { PORTA=0x00; DDRA=0x00; PORTB=0xFF; DDRB=0xFF; PORTC=0x00; DDRC=0x00; PORTD=0x00; DDRD=0xFC; // Timer/Counter 0 initialization // Clock source: System Clock // Clock value: 2000.000 kHz // Mode: Fast PWM top=0xFF // OC0 output: Disconnected TCCR0=0x4A; TCNT0=0x00; OCR0=0x0F; // Timer/Counter 1 initialization // Clock source: System Clock // Clock value: 250.000 kHz // Mode: Fast PWM top=0x00FF // OC1A output: Non-Inv. // OC1B output: Non-Inv. // Noise Canceler: Off // Input Capture on Falling Edge // Timer1 Overflow Interrupt: Off // Input Capture Interrupt: Off // Compare A Match Interrupt: Off // Compare B Match Interrupt: Off TCCR1A=0xA1; TCCR1B=0x0B; TCNT1H=0x00; TCNT1L=0x00; ICR1H=0x00; ICR1L=0x00; OCR1AH=0x00; OCR1AL=0x00; OCR1BH=0x00; OCR1BL=0x00; // Timer/Counter 2 initialization // Clock source: System Clock // Clock value: Timer2 Stopped // Mode: Normal top=0xFF // OC2 output: Disconnected ASSR=0x00; TCCR2=0x00; TCNT2=0x00; OCR2=0x00; // External Interrupt(s) initialization // INT0: Off // INT1: Off // INT2: Off MCUCR=0x00; MCUCSR=0x00; // Timer(s)/Counter(s) Interrupt(s) initialization TIMSK=0x00; // USART initialization // USART disabled UCSRB=0x00; // Analog Comparator initialization // Analog Comparator: Off // Analog Comparator Input Capture by Timer/Counter 1: Off ACSR=0x80; SFIOR=0x00; // ADC initialization // ADC Clock frequency: 125.000 kHz // ADC Voltage Reference: AVCC pin // Only the 8 most significant bits of // the AD conversion result are used ADMUX=ADC_VREF_TYPE & 0xff; ADCSRA=0x87; // SPI initialization // SPI disabled SPCR=0x00; // TWI initialization // TWI disabled TWCR=0x00; // Alphanumeric LCD initialization // Connections are specified in the // Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu: // RS - PORTB Bit 0 // RD - PORTB Bit 1 // EN - PORTB Bit 2 // D4 - PORTB Bit 4 // D5 - PORTB Bit 5 // D6 - PORTB Bit 6 // D7 - PORTB Bit 7 // Characters/line: 16 lcd_init(16); lcd_clear(); define_char(fullBlock,FULL_BLOCK); define_char(emptyBlock,EMPTY_BLOCK); lcdOn(1); while (1) { } }