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) {
}
}
