#include "main.h"
// NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
// BAUD RATE = 9600
// univerzalni LED diody
#define LED1 PIN_A4
#define LED2 PIN_A5
// piezo pipak
#DEFINE SOUND_HI PIN_B4
#DEFINE SOUND_LO PIN_B5
// radkovy senzor
#define SDIN PIN_D4 // seriovy vstup
#define SDOUT input(PIN_B2) // seriovy vystup
#define SCLK PIN_D5 // takt
// pro komunikaci s OLSA, prvni se posila LSB
int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
int test[8]={1,0,1,0,1,0,1,0,};
int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
int mid_offset[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42
int right_offset[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44
int offset[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81
int left_gain[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41
int mid_gain[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43
int right_gain[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45
int gain[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5
int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08
int stop_int[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10
int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
int8 *line_lp=&olsa_lseg; // ukazatel na levou cast radky
int8 *line_rp=&olsa_rseg; // ukazatel na pravou cast radky
int8 pixel; // dec hodnota jednoho pixelu
int1 bpixel[8]={0}; // bin hodnota jednoho pixelu
int8 rpx; // pocet prectenych pixelu
int8 rbit; // pocet prectenych bitu
int8 cbit; // pocet prevedenych pixelu
//naraznik
#define BUMPL input(PIN_D6)
#define BUMPR input(PIN_D7)
//nouzove senzory
#define LINEL 0
#define LINER 1
#define TRESHOLD 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
int8 line_l;
int8 line_r;
int1 line_position;
// motory
#define LMF PIN_D0
#define LMB PIN_D1
#define RMF PIN_D2
#define RMB PIN_D3
int8 lm_speed;
int8 rm_speed;
//PODPROGRAMY
//SENZORY
//OLSA01A
void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
{
int8 ct;
for(ct=0;ct<=count;ct++)
{
output_high(SCLK);
output_low(SCLK);
}
}
void olsa_pulse() // vytvori jeden impulz
{
output_high(SCLK);
output_low(SCLK);
}
void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
{
int *ip; // ukazatel na pole s informaci
int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
output_low(SDIN); // start bit
olsa_pulse();
for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
{
i=info[ip]; // ziskani hodnoty z pole
if(i==1) // vyhodnoceni obsahu informace - nastav 1
{
output_high(SDIN);
}
else // vyhodnoceni obsahu informace - nastav 0
{
output_low(SDIN);
}
olsa_pulse();
}
output_high(SDIN); // stop bit
olsa_pulse();
}
void olsa_reset() // hlavni RESET - provadi se po zapnuti
{
output_low(SDIN);
output_low(SCLK);
olsa_pulses(30); // reset radkoveho senzoru
output_high(SDIN);
olsa_pulses(10); // start bit - synchronizace
olsa_send(main_reset);
olsa_pulses(5);
olsa_send(set_mode_rg);
olsa_send(clear_mode_rg);
}
void olsa_setup() // kompletni nastaveni, provadi se po resetu
{
olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
olsa_send(offset);
olsa_send(left_gain);
olsa_send(gain);
olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
olsa_send(offset);
olsa_send(mid_gain);
olsa_send(gain);
olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
olsa_send(offset);
olsa_send(right_gain);
olsa_send(gain);
}
void olsa_integration() // snimani pixelu
{
olsa_send(start_int); // zacatek integrace senzoru
olsa_pulses(22);
olsa_send(stop_int); // konec integrace senzoru
olsa_pulses(5);
}
void olsa_bit_save() // ukladani jednotlivych bitu pixelu
{
t_bit_save:
if(SDOUT==0) // zacatek prenosu
{
do // prijimej zpravy
{
if(SDOUT==1) // zapise do bitu 1
{
bpixel[rbit]=1;
}
else // zapise do bitu 0
{
bpixel[rbit]=0;
}
rbit++; // zapocita precteni bitu 0 - 7
}
while(rbit==7);
goto e_bit_save; // skoc na konec prenosu
}
else // posli impulz a cekej na prenos
{
olsa_pulse();
goto t_bit_save; // skoci na zacatek prenosu, pokud SDOUT == 1
}
e_bit_save:
olsa_pulse(); // posle impulz pro generovani stop bitu
}
void olsa_convert() // prevede pole prijmutych bitu na pixel
{
pixel=0; // vynuluje pixel
for(cbit=0;cbit<8;cbit++)
{
pixel|=bpixel[cbit]<<cbit; // prevadi bity do pixelu
}
}
void olsa_byte_save() // zapis pixelu do pole
{
if(rpx<=51) // leva polovina radky
{
olsa_lseg[line_lp]=pixel; // zapis do pole
line_lp++;
}
else // prava polovina radky
{
olsa_rseg[line_rp]=pixel; // zapis od pole
line_rp++;
}
}
void olsa_evaluate() // vyhodnoceni polohy
{
}
void olsa_read() // precist senzor
{
rpx=0; // cteny pixel = 0
line_lp=0; // ukazatel na levou cast radky = 0
line_rp=0; // ukazatel na pravou cast radky = 0
olsa_send(readout); // prikaz pro cteni ze senzoru
do
{
olsa_bit_save(); // precte a ulozi bity
olsa_convert(); // prevede bity do jednoho bytu
olsa_byte_save(); // zapise byte do pole
rpx++;
}
while(rpx==101);
olsa_evaluate(); // zjisti pozici cary
}
//ZACHRANNE SENZORY
void read_blue_sensors() // cteni nouzovych senzoru
{
set_adc_channel(LINEL); // cti levy nouzovy senzor
delay_us(10);
line_l=read_adc();
set_adc_channel(LINER); // cti pravy nouzovy senzor
delay_us(10);
line_r=read_adc();
}
//PIPAK
void beep(int16 period,int16 length)
{
int16 bp; //promenna pro nastaveni delky
for(bp=length;bp>0;bp--)
{
output_high(SOUND_HI);
output_low(SOUND_LO);
delay_us(period);
output_high(SOUND_LO);
output_low(SOUND_HI);
delay_us(period);
}
}
//MOTORY
void l_motor_fwd(int8 speedl) // levy motor dopredu
{
output_high(LMF);
output_low(LMB);
set_pwm2_duty(speedl);
}
void l_motor_bwd(int8 speedl) // levy motor dozadu
{
output_high(LMB);
output_low(LMF);
set_pwm2_duty(speedl);
}
void r_motor_fwd(int8 speedr) // pravy motor dopredu
{
output_high(RMF);
output_low(RMB);
set_pwm1_duty(speedr);
}
void r_motor_bwd(int8 speedr) // pravy motor dozadu
{
output_high(RMB);
output_low(RMF);
set_pwm1_duty(speedr);
}
void l_motor_off() // levy motor vypnut
{
output_low(LMF);
output_low(LMB);
set_pwm2_duty(0);
}
void r_motor_off() // pravy motor vypnut
{
output_low(RMF);
output_low(RMB);
set_pwm1_duty(0);
}
void motor_test() // test motoru
{
int8 i;
beep(100,200);
printf("TEST MOTORU\r\n");
delay_ms(1000);
printf("LEVY MOTOR DOPREDU\r\n");
delay_ms(1000);
for(i=0;i<255;i++)
{
l_motor_fwd(i);
printf("RYCHLOST: %u\r\n",i);
delay_ms(5);
}
for(i=255;i>0;i--)
{
l_motor_fwd(i);
printf("RYCHLOST: %u\r\n",i);
delay_ms(5);
}
printf("LEVY MOTOR DOZADU\r\n");
delay_ms(1000);
for(i=0;i<255;i++)
{
l_motor_bwd(i);
printf("RYCHLOST: %u\r\n",i);
delay_ms(5);
}
for(i=255;i>0;i--)
{
l_motor_bwd(i);
printf("RYCHLOST: %u\r\n",i);
delay_ms(5);
}
printf("PRAVY MOTOR DOPREDU\r\n");
delay_ms(1000);
for(i=0;i<255;i++)
{
r_motor_fwd(i);
printf("RYCHLOST: %u\r\n",i);
delay_ms(5);
}
for(i=255;i>0;i--)
{
r_motor_fwd(i);
printf("RYCHLOST: %u\r\n",i);
delay_ms(5);
}
printf("PRAVY MOTOR DOZADU\r\n");
delay_ms(1000);
for(i=0;i<255;i++)
{
r_motor_bwd(i);
printf("RYCHLOST: %u\r\n",i);
delay_ms(5);
}
for(i=255;i>0;i--)
{
r_motor_bwd(i);
printf("RYCHLOST: %u\r\n",i);
delay_ms(5);
}
l_motor_off();
r_motor_off();
printf("KONEC TESTU MOTORU\r\n");
delay_ms(1000);
}
void diagnostika() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
{
read_blue_sensors();
printf("LEVA: %u \t",line_l);
delay_ms(10);
printf("PRAVA: %u \t",line_r);
delay_ms(10);
printf("L_NARAZ: %u \t",BUMPL);
delay_ms(10);
printf("P_NARAZ: %u \r\n",BUMPR);
delay_ms(10);
if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
{
motor_test();
}
}
// HLAVNI SMYCKA
void main()
{
printf("POWER ON \r\n");
// NASTAVENI > provede se pouze pri zapnuti
setup_adc_ports(sAN0-sAN1-sAN2);
setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
setup_spi(SPI_SS_DISABLED);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
setup_ccp2(CCP_PWM); // povolĂ PWM na pinu RC1
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
l_motor_off(); // vypne levy motor
r_motor_off(); // vypne pravy motor
olsa_reset(); // reset logiky radkoveho senzoru
olsa_setup(); // nastaveni segmentu radkoveho senzoru (offset a zisk)
output_high(LED1); // zhasne LED1
output_high(LED2); // zhasne LED2
beep(500,200); // pipni pri startu
printf("OK! \r\n");
delay_ms(500);
printf("VYBRAT MOD... \r\n");
while(true)
{
olsa_send(main_reset);
}
}