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

部ロボtransmit.アナログ値は適当に処理してから送信。

Dependencies:   mbed

main.cpp@0:f3297abb5739, 2015-01-10 (annotated)

Committer:
eil4nyqn
Date:
Sat Jan 10 04:30:31 2015 +0000
Revision:
0:f3297abb5739
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eil4nyqn 0:f3297abb5739 1 #include "mbed.h"
eil4nyqn 0:f3297abb5739 2
eil4nyqn 0:f3297abb5739 3 PwmOut mytest[] = {LED1,LED2,LED3,LED4};
eil4nyqn 0:f3297abb5739 4 AnalogIn stick[] = {p20,p19};
eil4nyqn 0:f3297abb5739 5 Serial pc(USBTX,USBRX);
eil4nyqn 0:f3297abb5739 6 Serial mc(p13,p14);
eil4nyqn 0:f3297abb5739 7 DigitalIn swith[] = {p5,p6,p7,p8};
eil4nyqn 0:f3297abb5739 8
eil4nyqn 0:f3297abb5739 9 int main() {
eil4nyqn 0:f3297abb5739 10 double analog_r,analog_l,analog_ra[10],analog_la[10];
eil4nyqn 0:f3297abb5739 11 int i;
eil4nyqn 0:f3297abb5739 12 uint8_t data_r1,data_r2,data_l1,data_l2,start = 255,check;
eil4nyqn 0:f3297abb5739 13 uint8_t sw_data1,sw_data2,sw_data3,sw_data4,sw_data5,sw_data6;
eil4nyqn 0:f3297abb5739 14 double signal_r1,signal_r2,signal_l1,signal_l2;
eil4nyqn 0:f3297abb5739 15 double ave_r,ave_l,sum_r,sum_l;
eil4nyqn 0:f3297abb5739 16 swith[0].mode(PullUp);
eil4nyqn 0:f3297abb5739 17 swith[1].mode(PullUp);
eil4nyqn 0:f3297abb5739 18 swith[2].mode(PullUp);
eil4nyqn 0:f3297abb5739 19 swith[3].mode(PullUp);
eil4nyqn 0:f3297abb5739 20 wait(0.25);
eil4nyqn 0:f3297abb5739 21 for(i=0;i<10;i++){
eil4nyqn 0:f3297abb5739 22 analog_ra[i] = stick[0];
eil4nyqn 0:f3297abb5739 23 analog_la[i] = stick[1];
eil4nyqn 0:f3297abb5739 24 sum_r += analog_ra[i];
eil4nyqn 0:f3297abb5739 25 sum_l += analog_la[i];
eil4nyqn 0:f3297abb5739 26 pc.printf("%f_%f\n",analog_ra[i],analog_la[i]);
eil4nyqn 0:f3297abb5739 27 wait(0.1);
eil4nyqn 0:f3297abb5739 28 }
eil4nyqn 0:f3297abb5739 29 ave_r = sum_r/10;
eil4nyqn 0:f3297abb5739 30 ave_l = sum_l/10;
eil4nyqn 0:f3297abb5739 31 mytest[0] = 1;
eil4nyqn 0:f3297abb5739 32 wait(0.2);
eil4nyqn 0:f3297abb5739 33
eil4nyqn 0:f3297abb5739 34 while(1) {
eil4nyqn 0:f3297abb5739 35 data_r1 = 0;
eil4nyqn 0:f3297abb5739 36 data_r2 = 0;
eil4nyqn 0:f3297abb5739 37 data_l1 = 0;
eil4nyqn 0:f3297abb5739 38 data_l2 = 0;
eil4nyqn 0:f3297abb5739 39 analog_r = stick[0];
eil4nyqn 0:f3297abb5739 40 analog_l = stick[1];
eil4nyqn 0:f3297abb5739 41
eil4nyqn 0:f3297abb5739 42 if(analog_r > ave_r-0.050 && analog_r < ave_r+0.050){
eil4nyqn 0:f3297abb5739 43 signal_r1 = 0;
eil4nyqn 0:f3297abb5739 44 signal_r2 = 0;
eil4nyqn 0:f3297abb5739 45 mytest[0] = signal_r1;
eil4nyqn 0:f3297abb5739 46 mytest[1] = signal_r2;
eil4nyqn 0:f3297abb5739 47 }else if(analog_r < ave_r-0.050){
eil4nyqn 0:f3297abb5739 48 signal_r1 = 0;
eil4nyqn 0:f3297abb5739 49 signal_r2 = (analog_r-ave_r)/ave_r*(-1);
eil4nyqn 0:f3297abb5739 50 data_r1 = (signal_r2+0.0005)*200;
eil4nyqn 0:f3297abb5739 51 mytest[0] = signal_r1;
eil4nyqn 0:f3297abb5739 52 mytest[1] = signal_r2;
eil4nyqn 0:f3297abb5739 53 }else if(analog_r > ave_r+0.050){
eil4nyqn 0:f3297abb5739 54 signal_r2 = 0;
eil4nyqn 0:f3297abb5739 55 signal_r1 = (analog_r-ave_r)/ave_r;
eil4nyqn 0:f3297abb5739 56 data_r2 = (signal_r1+0.0005)*200;
eil4nyqn 0:f3297abb5739 57 mytest[0] = signal_r1;
eil4nyqn 0:f3297abb5739 58 mytest[1] = signal_r2;
eil4nyqn 0:f3297abb5739 59 }
eil4nyqn 0:f3297abb5739 60 if(analog_l > ave_l-0.050 && analog_l < ave_l+0.050){
eil4nyqn 0:f3297abb5739 61 signal_l1 = 0;
eil4nyqn 0:f3297abb5739 62 signal_l2 = 0;
eil4nyqn 0:f3297abb5739 63 mytest[2] = signal_l1;
eil4nyqn 0:f3297abb5739 64 mytest[3] = signal_l2;
eil4nyqn 0:f3297abb5739 65 }else if(analog_l < ave_l-0.050){
eil4nyqn 0:f3297abb5739 66 signal_l1 = 0;
eil4nyqn 0:f3297abb5739 67 signal_l2 = (analog_l-ave_l)/ave_l*(-1);
eil4nyqn 0:f3297abb5739 68 data_l1 = (signal_l2+0.0005)*200;
eil4nyqn 0:f3297abb5739 69 mytest[2] = signal_l1;
eil4nyqn 0:f3297abb5739 70 mytest[3] = signal_l2;
eil4nyqn 0:f3297abb5739 71 }else if(analog_l > ave_l+0.050){
eil4nyqn 0:f3297abb5739 72 signal_l2 = 0;
eil4nyqn 0:f3297abb5739 73 signal_l1 = (analog_l-ave_l)/ave_l;
eil4nyqn 0:f3297abb5739 74 data_l2 = (signal_l1+0.0005)*200;
eil4nyqn 0:f3297abb5739 75 mytest[2] = signal_l1;
eil4nyqn 0:f3297abb5739 76 mytest[3] = signal_l2;
eil4nyqn 0:f3297abb5739 77 }
eil4nyqn 0:f3297abb5739 78
eil4nyqn 0:f3297abb5739 79 sw_data1 = !swith[0]*1;
eil4nyqn 0:f3297abb5739 80 sw_data2 = !swith[1]*2;
eil4nyqn 0:f3297abb5739 81 sw_data3 = !swith[2]*4;
eil4nyqn 0:f3297abb5739 82 sw_data4 = !swith[3]*8;
eil4nyqn 0:f3297abb5739 83 if(sw_data1 != 0 && sw_data2 == 0){
eil4nyqn 0:f3297abb5739 84 sw_data5 = sw_data1;
eil4nyqn 0:f3297abb5739 85 }else if(sw_data1 == 0 && sw_data2 != 0){
eil4nyqn 0:f3297abb5739 86 sw_data5 = sw_data2;
eil4nyqn 0:f3297abb5739 87 }else if(sw_data1 == 0 && sw_data2 == 0){
eil4nyqn 0:f3297abb5739 88 sw_data5 = 0;
eil4nyqn 0:f3297abb5739 89 }
eil4nyqn 0:f3297abb5739 90 if(sw_data3 != 0 && sw_data4 == 0){
eil4nyqn 0:f3297abb5739 91 sw_data6 = sw_data3;
eil4nyqn 0:f3297abb5739 92 }else if(sw_data3 == 0 && sw_data4 != 0){
eil4nyqn 0:f3297abb5739 93 sw_data6 = sw_data4;
eil4nyqn 0:f3297abb5739 94 }else if(sw_data3 == 0 && sw_data4 == 0){
eil4nyqn 0:f3297abb5739 95 sw_data6 = 0;
eil4nyqn 0:f3297abb5739 96 }
eil4nyqn 0:f3297abb5739 97 check = data_r1^data_r2^data_l1^data_l2^sw_data5^sw_data6;
eil4nyqn 0:f3297abb5739 98
eil4nyqn 0:f3297abb5739 99 mc.putc(start);
eil4nyqn 0:f3297abb5739 100 mc.putc(data_r1);
eil4nyqn 0:f3297abb5739 101 mc.putc(data_r2);
eil4nyqn 0:f3297abb5739 102 mc.putc(data_l1);
eil4nyqn 0:f3297abb5739 103 mc.putc(data_l2);
eil4nyqn 0:f3297abb5739 104 mc.putc(sw_data5);
eil4nyqn 0:f3297abb5739 105 mc.putc(sw_data6);
eil4nyqn 0:f3297abb5739 106 mc.putc(check);
eil4nyqn 0:f3297abb5739 107
eil4nyqn 0:f3297abb5739 108 pc.printf("%u_%u_%u_%u_%u_%u_%f_%f_%f_%f_%u\n",data_r1,data_r2,data_l1,data_l2,sw_data5,sw_data6,ave_r,ave_l,analog_r,analog_l,check);
eil4nyqn 0:f3297abb5739 109 }
eil4nyqn 0:f3297abb5739 110 }