1,6 → 1,6 |
//******** Mrakomer ****************************************** |
//******** Robot Camerus pro IstRobot 2007 ************ |
//"$Id$" |
//************************************************************ |
//***************************************************** |
|
#include ".\camerus.h" |
|
19,6 → 19,8 |
#define SERVO PIN_B4 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem)) |
#define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2 |
#define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1 |
#define MOT_1 PIN_C1 // PWM vystpy motoru |
#define MOT_2 PIN_C2 // |
#define DATA PIN_B2 // K modulu LEDbar data |
#define CP PIN_B1 // K modulu LEDbar hodiny |
#define ODO PIN_A4 // Ze snimace z odometrie z praveho kola |
43,11 → 45,39 |
stavy stav; // Kde jsme na trati |
int8 cas; // Cas hrany bila/cerna v radce |
|
|
inline void brzda() |
{ |
int8 n,i; |
|
set_pwm1_duty(0); // vypni PWM |
set_pwm2_duty(0); |
setup_ccp1(CCP_OFF); |
setup_ccp2(CCP_OFF); |
for (n=0;n<255;n++) |
{ |
output_low(MOT_L); |
output_low(MOT_R); |
output_high(MOT_1); |
output_high(MOT_2); |
delay_us(800); |
output_high(MOT_L); |
output_high(MOT_R); |
output_low(MOT_1); |
output_low(MOT_2); |
delay_us(800); |
} |
output_low(MOT_L); // smer vpred |
output_low(MOT_R); |
setup_ccp1(CCP_PWM); // RC1 // PWM pro motory |
setup_ccp2(CCP_PWM); // RC2 |
} |
|
void SetServo(int8 angle) |
{ |
int8 n, offset; |
|
for(n=0; n<20; n++) |
for(n=0; n<14; n++) |
{ |
set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka |
Delay_ms(1); |
68,35 → 98,46 |
#int_EXT |
EXT_isr() // Preruseni od prekazky |
{ |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_high(MOT_L); // vzad |
output_high(MOT_R); |
// Ujistime se, ze prijaty signal je z naseho IR vysilace |
output_high(IRTX); // Vypni LED na detekci prekazky |
set_pwm1_duty(0); // Vypni motory |
set_pwm2_duty(0); |
output_low(MOT_L); |
delay_ms(2); |
if (!input(IRRX)) // stale nas signal? |
{ |
output_low(MOT_L); // je odraz -> vpred |
output_low(MOT_R); |
delay_ms(2); |
if (!input(IRRX)) return; |
return; |
}; |
output_low(IRTX); // Zapni LED na detekci prekazky |
delay_ms(10); |
if (input(IRRX)) return; |
if (input(IRRX)) // stale nas signal? |
{ |
output_low(MOT_L); // neni odraz -> vpred |
output_low(MOT_R); |
return; |
}; |
|
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_high(MOT_L); |
output_high(MOT_R); |
delay_ms(400); |
output_low(MOT_L); // zastav |
output_low(MOT_R); |
if (stav==cihla) while(true); // Zastav na furt |
if(stav==jizda) |
SetServo((CASAVR-CASMIN)); // rovne |
delay_ms(100); // Doba reverzace |
|
brzda(); |
|
if (stav==cihla) while(true); // Zastav na furt, konec drahy |
|
if(stav==jizda) // Objed cihlu |
{ |
int n; |
/* |
SetServo(CASAVR-CASMIN); |
set_pwm1_duty(40); // pomalu couvej |
set_pwm2_duty(40); |
output_high(MOT_L); |
output_high(MOT_R); |
|
SetServo(CASMIN); // prudce doleva |
delay_ms(200); |
|
set_pwm1_duty(150); // vpred |
set_pwm2_duty(200); |
output_low(MOT_L); |
output_low(MOT_R); |
n=0; |
while(true) |
{ |
103,26 → 144,18 |
while(input(ODO)); |
while(!input(ODO)); |
n++; |
if(n==6) break; |
if(n==5) break; |
} |
set_pwm1_duty(255); // reverz (zabrzdi) |
set_pwm2_duty(255); |
output_low(MOT_L); |
output_low(MOT_R); |
delay_ms(100); |
set_pwm1_duty(0); // Zastav |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_low(MOT_R); |
*/ |
delay_ms(1000); |
output_high(MOT_L); |
output_high(MOT_R); |
|
SetServo((CASAVR-CASMIN)-20); // doleva |
set_pwm1_duty(150); // vpred |
set_pwm2_duty(200); |
SetServo((CASAVR-CASMIN)); // rovne |
set_pwm1_duty(140); // vpred |
set_pwm2_duty(140); |
output_low(MOT_L); |
output_low(MOT_R); |
stav=cihla; |
n=0; |
while(true) |
{ |
129,19 → 162,16 |
while(input(ODO)); |
while(!input(ODO)); |
n++; |
if(n==10) break; |
if(n==6) break; |
} |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_high(MOT_L); |
output_high(MOT_R); |
|
SetServo((CASAVR-CASMIN)); // rovne |
brzda(); |
|
SetServo((CASAVR-CASMIN)+70); // doprava |
set_pwm1_duty(140); // vpred |
set_pwm2_duty(140); |
output_low(MOT_L); |
output_low(MOT_R); |
stav=cihla; |
n=0; |
while(true) |
{ |
148,25 → 178,22 |
while(input(ODO)); |
while(!input(ODO)); |
n++; |
if(n==10) break; |
if(n==6) break; |
} |
|
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_high(MOT_L); |
output_high(MOT_R); |
delay_ms(200); |
set_pwm1_duty(150); // Zastav |
set_pwm2_duty(150); |
output_high(MOT_L); |
output_high(MOT_R); |
delay_ms(1000); |
delay_ms(100); |
|
brzda(); |
|
SetServo(CASMIN); // max. doleva |
set_pwm1_duty(0); // vzad |
set_pwm2_duty(20); |
output_low(MOT_L); |
output_high(MOT_R); |
stav=cihla; |
n=0; |
while(true) |
{ |
173,7 → 200,7 |
while(input(ODO)); |
while(!input(ODO)); |
n++; |
if(n==10) break; |
if(n==5) break; |
} |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(255); |
180,19 → 207,14 |
output_low(MOT_L); |
output_low(MOT_R); |
delay_ms(100); |
set_pwm1_duty(0); // Zastav |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_low(MOT_R); |
delay_ms(500); |
|
brzda(); |
|
SetServo((CASAVR-CASMIN)+5); // mirne doprava |
set_pwm1_duty(180); // vpred |
set_pwm2_duty(180); |
SetServo((CASAVR-CASMIN)+20); // mirne doprava |
set_pwm1_duty(190); // vpred |
set_pwm2_duty(190); |
output_low(MOT_L); |
output_low(MOT_R); |
stav=cihla; |
n=0; |
while(true) |
{ |
199,15 → 221,17 |
while(input(ODO)); |
while(!input(ODO)); |
n++; |
if(n==12) break; |
if(n==14) break; |
} |
set_pwm1_duty(0); // Zastav |
/* |
set_pwm1_duty(0); // reverz (zabrzdi) |
set_pwm2_duty(0); |
output_low(MOT_L); |
output_low(MOT_R); |
delay_ms(500); |
output_high(MOT_L); |
output_high(MOT_R); |
delay_ms(100); |
*/ |
brzda(); |
|
|
cas=CASMIN; // Cara je vlevo |
|
stav=cihla; |
344,7 → 368,7 |
i2c_stop(); |
delay_ms(50); |
|
for(offset=0x60;offset<=254;offset+=0x04) // Zacni od jasu 60h |
for(offset=0x60;offset<(255-0x04);offset+=0x04) // Zacni od jasu 60h |
{ |
i2c_start(); // Brightness |
i2c_write(0xC0); |
378,11 → 402,12 |
delay_ms(1); |
offset=read_adc()>>1; // 0-127 |
offset &= 0b11111100; // Dva nejnizsi bity ignoruj |
offset += 0x70; // Jas nebude nikdy nizsi |
disp(offset); |
i2c_start(); // Brightness |
i2c_write(0xC0); |
i2c_write(0x06); |
i2c_write(offset+0x70); // 80h default |
i2c_write(offset); // 80h default |
i2c_stop(); |
delay_ms(1000); // Nech hodnotu chvili na displayi |
|
475,7 → 500,7 |
set_pwm1_duty(255); // Rychly rozjezd !!! Zkontrolovat na oscyloskopu |
set_pwm2_duty(255); |
disp(0x1); |
delay_ms(200); |
delay_ms(400); |
stav=jizda; |
} |
|