![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Fork of JakKrisowy_ by
Revision 2:d1dec651ff13, committed 2016-07-19
- Comitter:
- jguzik
- Date:
- Tue Jul 19 13:10:16 2016 +0000
- Parent:
- 1:1e01b12063cd
- Commit message:
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Jul 14 13:31:01 2016 +0000 +++ b/main.cpp Tue Jul 19 13:10:16 2016 +0000 @@ -16,9 +16,10 @@ //*****deklaracja zmiennych***** -const int BufferSize=15; +const int BufferSize=18; char buffer1[BufferSize]; char buffer2[BufferSize]; +char buffer3[BufferSize]; char camove[2]; char tablicaf[4]; int wall=30; // odleglosc od sciany w cm @@ -42,14 +43,23 @@ } int what_command(char* buffer1){ // rozpoznajaca komende - if ((buffer1[0]=='f') && (distance>30)) - return 1; - else if ((buffer1[0]=='t') && (distance>30)) - if (buffer1[2]=='l') - return 2; - else - return 3; - else if (buffer1[0]=='b') + + /*forward 1 + t_leftt 2 + t_right 3 + backkkk 4 + STOPPPP 5 + leftttt 6 + righttt 7 + v_m_lef 8 + h_m_lef 9 + v_m_rig 10 + h_m_rig 11 + _m_zero 12 + unknown command*/ + + + if (buffer1[0]=='b') return 4; else if (buffer1[0]=='S') return 5; @@ -57,18 +67,37 @@ return 6; else if (buffer1[0]=='r') return 7; - else if (buffer1[0]=='v') + else if (buffer1[0]=='v'){ if (buffer1[4]=='l') return 8; else - return 10; - else if (buffer1[0]=='h') + return 10;} + else if (buffer1[0]=='h'){ if (buffer1[4]=='l') return 9; else - return 11; + return 11;} else if (buffer1[0]=='_') return 12; + else if (distance<30){ + if (buffer1[0]=='f') + return 5; + else if(buffer1[0]=='b') + return 4; + else if (buffer1[0]=='l') + return 6; + else if (buffer1[0]=='r') + return 7; + //return 5; + } + + else if ((buffer1[0]=='f')) + return 1; + else if ((buffer1[0]=='t')){ + if (buffer1[2]=='l') + return 2; + else + return 3;} else return 0; } @@ -145,7 +174,30 @@ sscanf(buffer2, "%3d", &val); } +void go_slower(){ + if(distance<50) + val=0.5*val; + if(distance<30) + val=0.2*val; + } + +void distance2array(int dist, char * dist_array){ + if(dist<10){ + dist_array[0]='0'; + dist_array[1]='0'; + dist_array[2]=dist+48;} + else if(dist>=10 && dist<100){ + dist_array[0]='0'; + dist_array[1]=dist/10+48; + dist_array[2]=dist%10+48;} + else { + dist_array[0]=dist/100+48; + dist_array[1]=dist%100/10+48; + dist_array[2]=dist%10+48;} +} + int main(){ + char dist_array[3]; stm.baud(115200); cleanBuffer1(); cleanBuffer2(); @@ -156,12 +208,12 @@ while(true){ //stm.printf("Poczatek petli nr %d \n", i); - if(t.read_ms()>500) //jesli przez ponad 500ms nie ma nowej ramki, zatrzymujemy robota + if(t.read_ms()>200) //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota { eng_left.move(0); eng_right.move(0); } - if(t_sonar.read_ms()>1000) //odczyt odleglosci co 1,2s + if(t_sonar.read_ms()>100) //odczyt odleglosci co 0,4s { distance = sensor.distance(); //stm.printf("distance z czujnika: %d\n",distance); @@ -175,10 +227,10 @@ /*stm.gets(buffer1, 13); stm.puts(buffer1); break;}*/ - + for (int x=0; x<12; x++){ if(t_fidu.read_ms()>1000){ - stm.printf("urywam;"); + //stm.printf("urywam;"); t_fidu.stop(); t_fidu.reset(); break;} @@ -189,29 +241,54 @@ t_fidu.start(); break;} } - stm.puts(buffer1); + distance2array(distance, dist_array); + distance2array(distance, buffer3); + for(int i=0; i<3; i++){ + buffer3[i]=dist_array[i];} + for(int i=0; i<12; i++){ + buffer3[i+3]=buffer1[i];} + + + stm.puts(buffer3); break;} set_val(); - t.stop(); t.reset(); t.start(); switch (what_command(buffer1)) { + + /*forward 1 + t_leftt 2 + t_right 3 + backkkk 4 + STOPPPP 5 + leftttt 6 + righttt 7 + v_m_lef 8 + h_m_lef 9 + v_m_rig 10 + h_m_rig 11 + _m_zero 12 + unknown command*/ + case 1: + go_slower(); eng_left.move(val); eng_right.move(val); set_camera_zero(); speed_car = val; break; case 2: + go_slower(); eng_right.move(speed_car); eng_left.move(speed_car - map(val, 0, 100, 0, speed_car)); cam_horizontal.position(7); cam_vertical.position(-20); break; case 3: + go_slower(); eng_left.move(speed_car); eng_right.move(speed_car - map(val, 0, 100, 0, speed_car)); cam_horizontal.position(-23);