mbedを用いた制御学生の制御
/
burobo_reciverV2
部ロボ受信機のプログラムです。
main.cpp
- Committer:
- WAT34
- Date:
- 2015-01-10
- Revision:
- 1:b77a3bd2e93e
- Parent:
- 0:cedc1d2ef037
- Child:
- 2:5c47df2771ec
File content as of revision 1:b77a3bd2e93e:
#include "mbed.h" Timer t; Serial pc(USBTX,USBRX); Serial trans(p9,p10); DigitalOut myled(LED1); DigitalOut led2(LED2); DigitalOut trig(p19); DigitalIn echo(p20); BusOut mc(p23,p24,p25,p26); PwmOut m1(p21); PwmOut m2(p22); BusOut updown(p27,p28); void move(int a,int b,int c,int d) { int width = 5.0; int seiten = 10; int gyaku = 5; int l = 6; int r = 9; int stop = 0; double mp1,mp2,mp1b; mp1 = (c-1)/100.0; mp1b = (1-c)/100.0; mp2 = 1.0-((d-1)/100.0); //pc.printf("mp1*mp2--->%f\n\r",mp1*mp2); //pc.printf("mp1--->%f",mp1); if(d>b+width) { if(c>b+width) { mc = seiten; m1 = mp1; m2 = mp1*mp2; }else if(c<b-width) { mc = gyaku; m2 = 1-mp1; m1 = (1-mp1)*mp2; }else{ mc = l; m1 = 1-mp2; m2 = 1-mp2; } }else if(d < b -width){ if(c>b+width) { mc = seiten; m1 = 1-mp1*mp2; m2 = mp1; }else if(c<b-width) { mc = gyaku; m2 = 1-(1-mp1)*mp2; m1 = 1-mp1; }else{ mc = r; m1 = mp2; m2 = mp2; } }else { if(c>b+width) { mc = seiten; m1 = mp1; m2 = mp1; }else if(c<b-width) { mc = gyaku; m1 = 1-mp1; m2 = 1-mp1; }else{ mc = stop; m1 = 0; m2 = 0; } } } void up_down(int a,int b) { if (a == 0) { updown = 1; }else if (b == 0){ updown = 2; }else{ updown = 0; } } double sonic(double so) { t.reset(); trig = 1; wait_us(10); trig = 0; if(echo == 1){ t.start(); } if(echo == 0){ t.stop(); so = t.read(); so = (so/2.0)*34000.0; } return so; } int main() { double dis = 0; int8_t nx,ny,x,y,down_val,up_val; while(1) { if (trans.getc()== 255){ nx =trans.getc(); ny =trans.getc(); x = trans.getc(); y = trans.getc(); up_val = trans.getc(); down_val = trans.getc(); move(nx,ny,x,y); up_down(up_val,down_val); } } }