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

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

Dependencies:   mbed HCSR04

main.cpp@0:cedc1d2ef037, 2015-01-09 (annotated)

Committer:
WAT34
Date:
Fri Jan 09 08:57:41 2015 +0000
Revision:
0:cedc1d2ef037
Child:
1:b77a3bd2e93e
fff;

Who changed what in which revision?

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