Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 189 → Rev 190

/roboti/istrobot/camerus/SW/876/camerus.c
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);
output_low(MOT_R);
delay_ms(2);
if (!input(IRRX)) return;
if (!input(IRRX)) // stale nas signal?
{
output_low(MOT_L); // je odraz -> vpred
output_low(MOT_R);
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_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;
}