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

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

Dependencies:   mbed HCSR04

main.cpp@1:b77a3bd2e93e, 2015-01-10 (annotated)

Committer:
WAT34
Date:
Sat Jan 10 04:18:22 2015 +0000
Revision:
1:b77a3bd2e93e
Parent:
0:cedc1d2ef037
Child:
2:5c47df2771ec
hhhh;

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 Timer t;
WAT34 1:b77a3bd2e93e 4 Serial pc(USBTX,USBRX);
WAT34 0:cedc1d2ef037 5 Serial trans(p9,p10);
WAT34 0:cedc1d2ef037 6 DigitalOut myled(LED1);
WAT34 1:b77a3bd2e93e 7 DigitalOut led2(LED2);
WAT34 1:b77a3bd2e93e 8 DigitalOut trig(p19);
WAT34 1:b77a3bd2e93e 9 DigitalIn echo(p20);
WAT34 0:cedc1d2ef037 10 BusOut mc(p23,p24,p25,p26);
WAT34 0:cedc1d2ef037 11 PwmOut m1(p21);
WAT34 0:cedc1d2ef037 12 PwmOut m2(p22);
WAT34 0:cedc1d2ef037 13 BusOut updown(p27,p28);
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 1:b77a3bd2e93e 22 double mp1,mp2,mp1b;
WAT34 0:cedc1d2ef037 23 mp1 = (c-1)/100.0;
WAT34 1:b77a3bd2e93e 24 mp1b = (1-c)/100.0;
WAT34 0:cedc1d2ef037 25 mp2 = 1.0-((d-1)/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 0:cedc1d2ef037 49 m1 = 1-mp1*mp2;
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 1:b77a3bd2e93e 54 m2 = 1-(1-mp1)*mp2;
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 1:b77a3bd2e93e 92 double sonic(double so)
WAT34 1:b77a3bd2e93e 93 {
WAT34 1:b77a3bd2e93e 94 t.reset();
WAT34 1:b77a3bd2e93e 95 trig = 1;
WAT34 1:b77a3bd2e93e 96 wait_us(10);
WAT34 1:b77a3bd2e93e 97 trig = 0;
WAT34 1:b77a3bd2e93e 98 if(echo == 1){
WAT34 1:b77a3bd2e93e 99 t.start();
WAT34 1:b77a3bd2e93e 100 }
WAT34 1:b77a3bd2e93e 101 if(echo == 0){
WAT34 1:b77a3bd2e93e 102 t.stop();
WAT34 1:b77a3bd2e93e 103 so = t.read();
WAT34 1:b77a3bd2e93e 104 so = (so/2.0)*34000.0;
WAT34 1:b77a3bd2e93e 105 }
WAT34 1:b77a3bd2e93e 106 return so;
WAT34 1:b77a3bd2e93e 107 }
WAT34 0:cedc1d2ef037 108 int main() {
WAT34 1:b77a3bd2e93e 109 double dis = 0;
WAT34 0:cedc1d2ef037 110 int8_t nx,ny,x,y,down_val,up_val;
WAT34 0:cedc1d2ef037 111 while(1) {
WAT34 0:cedc1d2ef037 112 if (trans.getc()== 255){
WAT34 0:cedc1d2ef037 113 nx =trans.getc();
WAT34 0:cedc1d2ef037 114 ny =trans.getc();
WAT34 0:cedc1d2ef037 115 x = trans.getc();
WAT34 0:cedc1d2ef037 116 y = trans.getc();
WAT34 0:cedc1d2ef037 117 up_val = trans.getc();
WAT34 0:cedc1d2ef037 118 down_val = trans.getc();
WAT34 0:cedc1d2ef037 119 move(nx,ny,x,y);
WAT34 1:b77a3bd2e93e 120 up_down(up_val,down_val);
WAT34 0:cedc1d2ef037 121 }
WAT34 1:b77a3bd2e93e 122 }
WAT34 0:cedc1d2ef037 123 }