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

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

Dependencies:   mbed HCSR04

main.cpp@3:43e85adf8768, 2015-01-12 (annotated)

Committer:
WAT34
Date:
Mon Jan 12 01:02:24 2015 +0000
Revision:
3:43e85adf8768
Parent:
2:5c47df2771ec 
????4????????????????????

Who changed what in which revision?

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