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

部ロボ用recive.タクトスイッチ入力時なめらかっぽく上昇付き。

Dependencies:   mbed

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

Committer:
eil4nyqn
Date:
Sat Jan 10 04:25:57 2015 +0000
Revision:
0:7092b32c717c
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eil4nyqn 0:7092b32c717c 1 #include "mbed.h"
eil4nyqn 0:7092b32c717c 2
eil4nyqn 0:7092b32c717c 3 PwmOut moter[] = {p21,p22,p23,p24};
eil4nyqn 0:7092b32c717c 4 BusOut selecter(p5,p6,p7,p8,p9,p10,p11,p12);
eil4nyqn 0:7092b32c717c 5 Serial pc(USBTX,USBRX);
eil4nyqn 0:7092b32c717c 6 Serial mc(p13,p14);
eil4nyqn 0:7092b32c717c 7 #define pi 3.141592
eil4nyqn 0:7092b32c717c 8
eil4nyqn 0:7092b32c717c 9 int main() {
eil4nyqn 0:7092b32c717c 10 uint8_t data_r1,data_r2,data_l1,data_l2,sw_data5,sw_data6,start;
eil4nyqn 0:7092b32c717c 11 double signal_r,signal_l,signal_a1,signal_a2,angle1,angle2;
eil4nyqn 0:7092b32c717c 12 uint8_t select_r,select_l,select_all,check,checker;
eil4nyqn 0:7092b32c717c 13 int n,m,x,z;
eil4nyqn 0:7092b32c717c 14
eil4nyqn 0:7092b32c717c 15 while(1){
eil4nyqn 0:7092b32c717c 16 select_r = 0;
eil4nyqn 0:7092b32c717c 17 select_l = 0;
eil4nyqn 0:7092b32c717c 18 signal_r = 0;
eil4nyqn 0:7092b32c717c 19 signal_l = 0;
eil4nyqn 0:7092b32c717c 20 signal_a1 = 0;
eil4nyqn 0:7092b32c717c 21 signal_a2 = 0;
eil4nyqn 0:7092b32c717c 22 if(mc.readable() == 1){
eil4nyqn 0:7092b32c717c 23 start = mc.getc();
eil4nyqn 0:7092b32c717c 24 data_r1 = mc.getc();
eil4nyqn 0:7092b32c717c 25 data_r2 = mc.getc();
eil4nyqn 0:7092b32c717c 26 data_l1 = mc.getc();
eil4nyqn 0:7092b32c717c 27 data_l2 = mc.getc();
eil4nyqn 0:7092b32c717c 28 sw_data5 = mc.getc();
eil4nyqn 0:7092b32c717c 29 sw_data6 = mc.getc();
eil4nyqn 0:7092b32c717c 30 check = mc.getc();
eil4nyqn 0:7092b32c717c 31
eil4nyqn 0:7092b32c717c 32 if(start == 255){
eil4nyqn 0:7092b32c717c 33 if(data_r1 != 0){
eil4nyqn 0:7092b32c717c 34 signal_r = data_r1/200.0000;
eil4nyqn 0:7092b32c717c 35 select_r = 16;
eil4nyqn 0:7092b32c717c 36 }else if(data_r2 != 0){
eil4nyqn 0:7092b32c717c 37 signal_r = data_r2/200.0000;
eil4nyqn 0:7092b32c717c 38 select_r = 32;
eil4nyqn 0:7092b32c717c 39 }
eil4nyqn 0:7092b32c717c 40 if(data_l1 != 0){
eil4nyqn 0:7092b32c717c 41 signal_l = data_l1/200.0000;
eil4nyqn 0:7092b32c717c 42 select_l = 64;
eil4nyqn 0:7092b32c717c 43 }else if(data_l2 != 0){
eil4nyqn 0:7092b32c717c 44 signal_l = data_l2/200.0000;
eil4nyqn 0:7092b32c717c 45 select_l = 128;
eil4nyqn 0:7092b32c717c 46 }
eil4nyqn 0:7092b32c717c 47 checker = data_r1^data_r2^data_l1^data_l2^sw_data5^sw_data6;
eil4nyqn 0:7092b32c717c 48 select_all = sw_data5+sw_data6+select_r+select_l;
eil4nyqn 0:7092b32c717c 49
eil4nyqn 0:7092b32c717c 50 if(sw_data5 == 1){
eil4nyqn 0:7092b32c717c 51 n += 8;
eil4nyqn 0:7092b32c717c 52 z = -90;
eil4nyqn 0:7092b32c717c 53 }else if(sw_data5/2 == 1){
eil4nyqn 0:7092b32c717c 54 z += 8;
eil4nyqn 0:7092b32c717c 55 n = -90;
eil4nyqn 0:7092b32c717c 56 }else if(sw_data5 == 0){
eil4nyqn 0:7092b32c717c 57 n = -90;
eil4nyqn 0:7092b32c717c 58 z = -90;
eil4nyqn 0:7092b32c717c 59 }
eil4nyqn 0:7092b32c717c 60 if((n+z+90) > -90 && (n+z+90) < 0){
eil4nyqn 0:7092b32c717c 61 angle1 = (n+z+90)*pi/180;
eil4nyqn 0:7092b32c717c 62 signal_a1 = (1.000+sin(angle1))/2;
eil4nyqn 0:7092b32c717c 63 }else if((n+z+90) == 0){
eil4nyqn 0:7092b32c717c 64 signal_a1 = 0.500;
eil4nyqn 0:7092b32c717c 65 }else if((n+z+90) > 0 && (n+z+90) < 90){
eil4nyqn 0:7092b32c717c 66 angle1 = (n+z+90)*pi/180.0;
eil4nyqn 0:7092b32c717c 67 signal_a1 = sin(angle1)/2+0.5000;
eil4nyqn 0:7092b32c717c 68 }else if((n+z+90) > 89){
eil4nyqn 0:7092b32c717c 69 signal_a1 = 1.000;
eil4nyqn 0:7092b32c717c 70 }else if(n == -90 && z == -90){
eil4nyqn 0:7092b32c717c 71 signal_a1 = 0;
eil4nyqn 0:7092b32c717c 72 angle1 = 0;
eil4nyqn 0:7092b32c717c 73 }
eil4nyqn 0:7092b32c717c 74 if(sw_data6/4 == 1){
eil4nyqn 0:7092b32c717c 75 m += 8;
eil4nyqn 0:7092b32c717c 76 x = -90;
eil4nyqn 0:7092b32c717c 77 }else if(sw_data6/8 == 1){
eil4nyqn 0:7092b32c717c 78 x += 8;
eil4nyqn 0:7092b32c717c 79 m = -90;
eil4nyqn 0:7092b32c717c 80 }else if(sw_data6 == 0){
eil4nyqn 0:7092b32c717c 81 m = -90;
eil4nyqn 0:7092b32c717c 82 x = -90;
eil4nyqn 0:7092b32c717c 83 }
eil4nyqn 0:7092b32c717c 84 if(m > -90 && m < 0 || x > -90 && x < 0){
eil4nyqn 0:7092b32c717c 85 angle2 = (m+x+90)*pi/180;
eil4nyqn 0:7092b32c717c 86 signal_a2 = (1.000+sin(angle2))/2;
eil4nyqn 0:7092b32c717c 87 }else if((m+x+90) == 0){
eil4nyqn 0:7092b32c717c 88 signal_a2 = 0.500;
eil4nyqn 0:7092b32c717c 89 }else if(m > 0 && m < 90 || x > 0 && x < 90){
eil4nyqn 0:7092b32c717c 90 angle2 = (m+x+90)*pi/180.0;
eil4nyqn 0:7092b32c717c 91 signal_a2 = sin(angle2)/2+0.5000;
eil4nyqn 0:7092b32c717c 92 }else if((m+x+90) > 89){
eil4nyqn 0:7092b32c717c 93 signal_a2 = 1.000;
eil4nyqn 0:7092b32c717c 94 }else if(m == -90 && x == -90){
eil4nyqn 0:7092b32c717c 95 signal_a2 = 0;
eil4nyqn 0:7092b32c717c 96 angle2 = 0;
eil4nyqn 0:7092b32c717c 97 }
eil4nyqn 0:7092b32c717c 98
eil4nyqn 0:7092b32c717c 99 pc.printf("%f_%f_%d_%d_%d_%d\n",signal_a1,signal_a2,n,z,m,x);
eil4nyqn 0:7092b32c717c 100 //pc.printf("%f_%f_%u_%u_%u_%u\n",signal_r,signal_l,select_r,select_l,select_all,check);
eil4nyqn 0:7092b32c717c 101 //pc.printf("%u_%u\n",checker,check);
eil4nyqn 0:7092b32c717c 102
eil4nyqn 0:7092b32c717c 103 if(check == checker){
eil4nyqn 0:7092b32c717c 104 moter[2] = signal_r;
eil4nyqn 0:7092b32c717c 105 moter[3] = signal_l;
eil4nyqn 0:7092b32c717c 106 moter[0] = signal_a1;
eil4nyqn 0:7092b32c717c 107 moter[1] = signal_a2;
eil4nyqn 0:7092b32c717c 108 selecter = select_all;
eil4nyqn 0:7092b32c717c 109 }
eil4nyqn 0:7092b32c717c 110 }
eil4nyqn 0:7092b32c717c 111 }
eil4nyqn 0:7092b32c717c 112 }
eil4nyqn 0:7092b32c717c 113 }