mbedを用いた制御学生の制御
/
burobo_reciverV2
部ロボ受信機のプログラムです。
main.cpp
- Committer:
- WAT34
- Date:
- 2015-01-09
- Revision:
- 0:cedc1d2ef037
- Child:
- 1:b77a3bd2e93e
File content as of revision 0:cedc1d2ef037:
#include "mbed.h" Serial trans(p9,p10); DigitalOut myled(LED1); 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; mp1 = (c-1)/100.0; mp2 = 1.0-((d-1)/100.0); 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 = 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 = 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; } } int main() { 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); } } }