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); } }
void sensors_loop() { if(sensor[0] < PROG && sensor[1] < PROG) // oba dolne { m1_start(FOR); m2_start(FOR); situation = 1; ticker = 0; } else if(sensor[2] < PROG && sensor[3] < PROG) // oba górne { m1_start(BCK); m2_start(BCK); situation = 2; ticker = 0; } else if(sensor[2] < PROG) //prawy przedni { m1_start(BCK); m2_start(BCK); situation = 5; ticker = 0; } else if(sensor[0] < PROG) //prawy tylni { m1_start(FOR); m2_start(FOR); situation = 6; ticker = 0; } else if(sensor[3] < PROG) //lewy przedni { m1_start(BCK); m2_start(BCK); situation = 7; ticker = 0; } else if(sensor[1] < PROG) //lewy tylni { m1_start(FOR); m2_start(FOR); situation = 8; ticker = 0; } else if(situation > 0) { switch(situation) { case 2: wait_ms(400); m1_start(FOR); m2_start(BCK); wait_ms(1000); break; case 5: case 6: wait_ms(400); m1_start(FOR); m2_start(BCK); wait_ms(600); break; case 7: case 8: wait_ms(400); m1_start(BCK); m2_start(FOR); wait_ms(600); break; } situation = 0; ticker = 0; } else if(sensor[7] > 500 || (sensor[5] < 750 && sensor[4] < 850)) // dzida { m1_start(FOR); m2_start(FOR); ticker = 0; } else if(sensor[4] < 850) // kontruj { m1_start(BCK); m2_start(FOR); ticker = 0; } else if(sensor[5] < 750) { m1_start(FOR); m2_start(BCK); ticker = 0; } else { ticker++; if(ticker < DAL) { m1_start(FOR); m2_start(FOR); } else if(ticker == DAL) { side = rand()%2; times = 6+rand()%15; } else if(ticker < DAL + times) { if(side == 0) { m1_start(FOR); m2_start(BCK); } else { m1_start(BCK); m2_start(FOR); } } else ticker = 0; } }
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; // } // } //} }