mbedを用いた制御学生の制御 / Mbed 2 deprecated burobo_reciverV2

部ロボ受信機のプログラムです。

Dependencies:   mbed HCSR04

main.cpp@2:5c47df2771ec, 2015-01-10 (annotated)

Committer:
WAT34
Date:
Sat Jan 10 05:30:58 2015 +0000
Revision:
2:5c47df2771ec
Parent:
1:b77a3bd2e93e
Child:
3:43e85adf8768
???????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WAT34 0:cedc1d2ef037 1 #include "mbed.h"
WAT34 0:cedc1d2ef037 2
WAT34 1:b77a3bd2e93e 3 Serial pc(USBTX,USBRX);
WAT34 0:cedc1d2ef037 4 Serial trans(p9,p10);
WAT34 0:cedc1d2ef037 5 DigitalOut myled(LED1);
WAT34 1:b77a3bd2e93e 6 DigitalOut led2(LED2);
WAT34 0:cedc1d2ef037 7 BusOut mc(p23,p24,p25,p26);
WAT34 0:cedc1d2ef037 8 PwmOut m1(p21);
WAT34 0:cedc1d2ef037 9 PwmOut m2(p22);
WAT34 0:cedc1d2ef037 10 BusOut updown(p27,p28);
WAT34 0:cedc1d2ef037 11 void move(int a,int b,int c,int d)
WAT34 0:cedc1d2ef037 12 {
WAT34 0:cedc1d2ef037 13 int width = 5.0;
WAT34 0:cedc1d2ef037 14 int seiten = 10;
WAT34 0:cedc1d2ef037 15 int gyaku = 5;
WAT34 0:cedc1d2ef037 16 int l = 6;
WAT34 0:cedc1d2ef037 17 int r = 9;
WAT34 0:cedc1d2ef037 18 int stop = 0;
WAT34 2:5c47df2771ec 19 double mp1,mp2;
WAT34 0:cedc1d2ef037 20 mp1 = (c-1)/100.0;
WAT34 0:cedc1d2ef037 21 mp2 = 1.0-((d-1)/100.0);
WAT34 1:b77a3bd2e93e 22 //pc.printf("mp1*mp2--->%f\n\r",mp1*mp2);
WAT34 1:b77a3bd2e93e 23 //pc.printf("mp1--->%f",mp1);
WAT34 0:cedc1d2ef037 24 if(d>b+width)
WAT34 0:cedc1d2ef037 25 {
WAT34 0:cedc1d2ef037 26 if(c>b+width)
WAT34 0:cedc1d2ef037 27 {
WAT34 0:cedc1d2ef037 28 mc = seiten;
WAT34 0:cedc1d2ef037 29 m1 = mp1;
WAT34 0:cedc1d2ef037 30 m2 = mp1*mp2;
WAT34 0:cedc1d2ef037 31 }else if(c<b-width)
WAT34 0:cedc1d2ef037 32 {
WAT34 0:cedc1d2ef037 33 mc = gyaku;
WAT34 0:cedc1d2ef037 34 m2 = 1-mp1;
WAT34 1:b77a3bd2e93e 35 m1 = (1-mp1)*mp2;
WAT34 0:cedc1d2ef037 36 }else{
WAT34 0:cedc1d2ef037 37 mc = l;
WAT34 0:cedc1d2ef037 38 m1 = 1-mp2;
WAT34 0:cedc1d2ef037 39 m2 = 1-mp2;
WAT34 0:cedc1d2ef037 40 }
WAT34 0:cedc1d2ef037 41 }else if(d < b -width){
WAT34 0:cedc1d2ef037 42 if(c>b+width)
WAT34 0:cedc1d2ef037 43 {
WAT34 0:cedc1d2ef037 44 mc = seiten;
WAT34 0:cedc1d2ef037 45 m1 = 1-mp1*mp2;
WAT34 0:cedc1d2ef037 46 m2 = mp1;
WAT34 0:cedc1d2ef037 47 }else if(c<b-width)
WAT34 0:cedc1d2ef037 48 {
WAT34 0:cedc1d2ef037 49 mc = gyaku;
WAT34 1:b77a3bd2e93e 50 m2 = 1-(1-mp1)*mp2;
WAT34 0:cedc1d2ef037 51 m1 = 1-mp1;
WAT34 0:cedc1d2ef037 52 }else{
WAT34 0:cedc1d2ef037 53 mc = r;
WAT34 0:cedc1d2ef037 54 m1 = mp2;
WAT34 0:cedc1d2ef037 55 m2 = mp2;
WAT34 0:cedc1d2ef037 56 }
WAT34 0:cedc1d2ef037 57 }else
WAT34 0:cedc1d2ef037 58 {
WAT34 0:cedc1d2ef037 59 if(c>b+width)
WAT34 0:cedc1d2ef037 60 {
WAT34 0:cedc1d2ef037 61 mc = seiten;
WAT34 0:cedc1d2ef037 62 m1 = mp1;
WAT34 0:cedc1d2ef037 63 m2 = mp1;
WAT34 0:cedc1d2ef037 64 }else if(c<b-width)
WAT34 0:cedc1d2ef037 65 {
WAT34 0:cedc1d2ef037 66 mc = gyaku;
WAT34 0:cedc1d2ef037 67 m1 = 1-mp1;
WAT34 0:cedc1d2ef037 68 m2 = 1-mp1;
WAT34 0:cedc1d2ef037 69 }else{
WAT34 0:cedc1d2ef037 70 mc = stop;
WAT34 0:cedc1d2ef037 71 m1 = 0;
WAT34 0:cedc1d2ef037 72 m2 = 0;
WAT34 0:cedc1d2ef037 73 }
WAT34 0:cedc1d2ef037 74 }
WAT34 0:cedc1d2ef037 75 }
WAT34 0:cedc1d2ef037 76 void up_down(int a,int b)
WAT34 0:cedc1d2ef037 77 {
WAT34 0:cedc1d2ef037 78 if (a == 0)
WAT34 0:cedc1d2ef037 79 {
WAT34 0:cedc1d2ef037 80 updown = 1;
WAT34 0:cedc1d2ef037 81 }else
WAT34 0:cedc1d2ef037 82 if (b == 0){
WAT34 0:cedc1d2ef037 83 updown = 2;
WAT34 0:cedc1d2ef037 84 }else{
WAT34 0:cedc1d2ef037 85 updown = 0;
WAT34 0:cedc1d2ef037 86 }
WAT34 0:cedc1d2ef037 87 }
WAT34 2:5c47df2771ec 88
WAT34 0:cedc1d2ef037 89 int main() {
WAT34 0:cedc1d2ef037 90 int8_t nx,ny,x,y,down_val,up_val;
WAT34 0:cedc1d2ef037 91 while(1) {
WAT34 0:cedc1d2ef037 92 if (trans.getc()== 255){
WAT34 0:cedc1d2ef037 93 nx =trans.getc();
WAT34 0:cedc1d2ef037 94 ny =trans.getc();
WAT34 0:cedc1d2ef037 95 x = trans.getc();
WAT34 0:cedc1d2ef037 96 y = trans.getc();
WAT34 0:cedc1d2ef037 97 up_val = trans.getc();
WAT34 0:cedc1d2ef037 98 down_val = trans.getc();
WAT34 0:cedc1d2ef037 99 move(nx,ny,x,y);
WAT34 1:b77a3bd2e93e 100 up_down(up_val,down_val);
WAT34 0:cedc1d2ef037 101 }
WAT34 2:5c47df2771ec 102
WAT34 1:b77a3bd2e93e 103 }
WAT34 0:cedc1d2ef037 104 }