void pad_loop()
{
int tmpl = 0, tmpr = 0;
bool back = false;
if(tab[8] > 128) // jeœli analog w dó³
{
tmpl = tab[8]-127;
tmpr = tmpl;
beep(500, 200);
back = true;
}
else if(tab[8] < 128) // jeœli w górê
{
tmpl = 128-tab[8];
tmpr = tmpl;
}
int p = 50;
if(tab[7] > 128) // jeœli analog w prawo
{
tmpr -= (p*(tab[7]-127))/100;
}
else if(tab[7] < 128) // jeœli w lewo
{
tmpl -= (p*(127-tab[7]))/100;
}
if(!(tab[4] & (1 << 3))) // jeœli wciœniêty R1 to turbo lvl1
{
if(tab[8] == 128) // jesli stoimy i chcemy zrobic baczka
{
if(tab[7] > 128) // jeœli analog w prawo
{
tmpl = -1 * tmpr;
}
else if(tab[7] < 128) // jeœli w lewo
{
tmpr = -1 * tmpl;
}
}
else
{
tmpl += 47;
tmpr += 47;
}
}
if(!(tab[4] & (1 << 1))) // jeœli wciœniêty R2 to turbo lvl2
{
tmpl += 80;
tmpr += 80;
}
if(back)
{
tmpl *= -1;
tmpr *= -1;
}
if(tab[8] == 128 && tab[7] == 128)
{
m1_stop();
m2_stop();
}
else
{
tmpl = tmpl * 100 / 255;
tmpr = tmpr * 100 / 255;
m1_set(tmpl);
m2_set(tmpr);
m1_start();
m2_start();
}
if(!(tab[4] & (1 << 6))) // jeœli wciœniêty X to klakson
{
beep(4000, 200);
}
}
int main()
{
unsigned char adress;
unsigned char primitive;
unsigned char trash;
pwm_init();
motor_init();
sei();
//m2_start();
//m1_start();
uart << "**** BSRM01 v.0.00001 ready. ****\r\n";
while(1){
//wait();
//uart >> adress;
//adress-=48;
//if (adress == MY_ID){
wait();
uart >> primitive;
switch (primitive){
case 'S':
m1_stop();
m2_stop();
uart << "BSRM01: M1, M2 STOP!";
case 'E':
unsigned char eng;
unsigned char power;
wait();
uart >> eng;
wait();
uart >> power;
if (eng == 0){
uart <<"E0:" << (int)power <<"%";
m1_set(power);
m1_start();
}
else if (eng == 1){
uart <<"E1:" << (int)power <<"%";
m2_set(power);
m2_start();
}
break;
}
}
// else {
// uart << "Are you talking to me, dude?\r\n";
// wait();
// uart >> primitive;
// for(int i = 0; i < size(primitive); i++){
// wait();
// uart >> trash;
// }
// }
//}
}
