Bチーム / Mbed 2 deprecated 4wd_omni_simple2

中継機能つけた受け取りオムニ

Dependencies:   mbed MultiSerial

main.cpp@18:9cbd70819d2d, 2014-09-27 (annotated)

Committer:
Hatter
Date:
Sat Sep 27 00:46:43 2014 +0000
Revision:
18:9cbd70819d2d
Parent:
17:3c82070177e5
Child:
19:1305ce40f4e2
stable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hatter 0:fddd018033fb 1 /*
Hatter 0:fddd018033fb 2 * 4WD_OMNI_simple by Tomoki Hokida
Hatter 13:8a9d3855ab96 3 * Simple Type
Hatter 0:fddd018033fb 4 *
Hatter 0:fddd018033fb 5 * motor pins piar
Hatter 13:8a9d3855ab96 6 * motor(p13,p14):motor(p15,p16)
Hatter 13:8a9d3855ab96 7 * motor(p17,p18):motor(p19,p20)
Hatter 13:8a9d3855ab96 8 *
Hatter 13:8a9d3855ab96 9 * Serial Pins
Hatter 15:1c1ae4077900 10 * packet.arm[0]:arm
Hatter 13:8a9d3855ab96 11 * packet.leg[1]:omni
Hatter 0:fddd018033fb 12 * reset 0
Hatter 0:fddd018033fb 13 */
Hatter 0:fddd018033fb 14
Hatter 0:fddd018033fb 15 #include "mbed.h"
Hatter 6:26e5a520472d 16 #include "MultiSerial.h"
Hatter 0:fddd018033fb 17
Hatter 14:731c52eb333b 18 #define DATA_NUM 2 //DATA_NUM[byte]通信 この値はmemcpy使ってるんでxbee_packet構造体のメモリ数といい感じに合わせるようにしてください
Hatter 13:8a9d3855ab96 19 #define XBEE_KEY 0xAA //keycode
Hatter 9:f05f881226a3 20 #define ARM_KEY 0xAA
Hatter 3:1f1498403eec 21
Hatter 17:3c82070177e5 22 #define PWM 0.5
Hatter 15:1c1ae4077900 23
Hatter 17:3c82070177e5 24 #define PWM_L 0.5
Hatter 17:3c82070177e5 25 #define PWM_R 0.5
Hatter 17:3c82070177e5 26 #define PWM_U 0.5
Hatter 17:3c82070177e5 27 #define PWM_D 0.5
Hatter 12:6e2ee1c83ac1 28
Hatter 15:1c1ae4077900 29 #define R 0x4
Hatter 15:1c1ae4077900 30 #define L 0x1
Hatter 15:1c1ae4077900 31 #define U 0x2
Hatter 15:1c1ae4077900 32 #define D 0x8
Hatter 15:1c1ae4077900 33
Hatter 15:1c1ae4077900 34 #define R_t 0x10
Hatter 15:1c1ae4077900 35 #define L_t 0x40
Hatter 15:1c1ae4077900 36
Hatter 15:1c1ae4077900 37 #define TIME 0
Hatter 13:8a9d3855ab96 38
Hatter 9:f05f881226a3 39 BusOut check(LED1,LED2,LED3,LED4);
Hatter 13:8a9d3855ab96 40 BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20);
Hatter 12:6e2ee1c83ac1 41
Hatter 15:1c1ae4077900 42 PwmOut pwm[4]= {p21,p22,p23,p24};
Hatter 0:fddd018033fb 43
Hatter 9:f05f881226a3 44 Serial pc(USBTX,USBRX);
Hatter 0:fddd018033fb 45
Hatter 16:4b502070bea8 46 MultiSerial xbee(p28,p27);
Hatter 16:4b502070bea8 47 MultiSerial armMbed(p9,p10);
Hatter 0:fddd018033fb 48
Hatter 13:8a9d3855ab96 49 /* data structure */
Hatter 15:1c1ae4077900 50 typedef struct {
Hatter 6:26e5a520472d 51
Hatter 6:26e5a520472d 52 uint8_t arm[1];
Hatter 6:26e5a520472d 53 uint8_t leg;
Hatter 4:5da566692b79 54
Hatter 15:1c1ae4077900 55 } xbee_packet;
Hatter 6:26e5a520472d 56
Hatter 6:26e5a520472d 57 xbee_packet packet;
Hatter 6:26e5a520472d 58
Hatter 15:1c1ae4077900 59 uint8_t get_data[DATA_NUM]= {0};
Hatter 13:8a9d3855ab96 60
Hatter 18:9cbd70819d2d 61 uint8_t check_data[2]={0};
Hatter 18:9cbd70819d2d 62
Hatter 16:4b502070bea8 63
Hatter 13:8a9d3855ab96 64 /* Out put PC stdout function */
Hatter 17:3c82070177e5 65 void pc_print()
Hatter 17:3c82070177e5 66 {
Hatter 13:8a9d3855ab96 67 static int count=0;
Hatter 13:8a9d3855ab96 68
Hatter 15:1c1ae4077900 69 if(count==10e3) {
Hatter 13:8a9d3855ab96 70 pc.printf(" arm = %d" ,packet.arm[0]);
Hatter 13:8a9d3855ab96 71 pc.printf(" leg = %d" ,packet.leg);
Hatter 13:8a9d3855ab96 72 count=0;
Hatter 15:1c1ae4077900 73 }
Hatter 13:8a9d3855ab96 74 count++;
Hatter 13:8a9d3855ab96 75 }
Hatter 0:fddd018033fb 76
Hatter 18:9cbd70819d2d 77 uint8_t readable_ratio(){
Hatter 18:9cbd70819d2d 78
Hatter 18:9cbd70819d2d 79 int check = 0;
Hatter 18:9cbd70819d2d 80
Hatter 18:9cbd70819d2d 81 for(int i=0;i>=100;i++){
Hatter 18:9cbd70819d2d 82
Hatter 18:9cbd70819d2d 83 if(xbee.readable_check()==1){check++;}
Hatter 18:9cbd70819d2d 84
Hatter 18:9cbd70819d2d 85 wait_ms(1.0);
Hatter 18:9cbd70819d2d 86 }
Hatter 18:9cbd70819d2d 87 return check;
Hatter 18:9cbd70819d2d 88 }
Hatter 18:9cbd70819d2d 89
Hatter 17:3c82070177e5 90 void connect_check(void)
Hatter 17:3c82070177e5 91 {
Hatter 16:4b502070bea8 92 xbee.start_read();
Hatter 16:4b502070bea8 93 xbee.read_data(check_data,XBEE_KEY);
Hatter 18:9cbd70819d2d 94
Hatter 18:9cbd70819d2d 95 check = 0x01;
Hatter 18:9cbd70819d2d 96
Hatter 18:9cbd70819d2d 97 while(readable_ratio()>=50);
Hatter 18:9cbd70819d2d 98
Hatter 18:9cbd70819d2d 99 check = 0x03;
Hatter 18:9cbd70819d2d 100
Hatter 18:9cbd70819d2d 101 while(readable_ratio()<=50);
Hatter 17:3c82070177e5 102
Hatter 18:9cbd70819d2d 103 check = 0x07;
Hatter 18:9cbd70819d2d 104
Hatter 16:4b502070bea8 105 xbee.stop_read();
Hatter 16:4b502070bea8 106 xbee.start_write();
Hatter 17:3c82070177e5 107
Hatter 16:4b502070bea8 108 check_data[0]=0xCC;
Hatter 17:3c82070177e5 109
Hatter 16:4b502070bea8 110 xbee.write_data(check_data,XBEE_KEY);
Hatter 17:3c82070177e5 111
Hatter 16:4b502070bea8 112 wait_ms(10);
Hatter 17:3c82070177e5 113
Hatter 16:4b502070bea8 114 xbee.stop_write();
Hatter 17:3c82070177e5 115 }
Hatter 16:4b502070bea8 116
Hatter 15:1c1ae4077900 117 void stop_slowly(int count)
Hatter 15:1c1ae4077900 118 {
Hatter 15:1c1ae4077900 119
Hatter 15:1c1ae4077900 120 for(float i=1.0; i>=0; i-=0.1) {
Hatter 17:3c82070177e5 121
Hatter 15:1c1ae4077900 122 /*for(int j=0; j>=4; j++) {
Hatter 15:1c1ae4077900 123 pwm[j] = i;
Hatter 15:1c1ae4077900 124
Hatter 15:1c1ae4077900 125 }*/
Hatter 17:3c82070177e5 126
Hatter 17:3c82070177e5 127 pwm[0] = 0.1;
Hatter 17:3c82070177e5 128 pwm[1] = 0.1;
Hatter 17:3c82070177e5 129 pwm[2] = 0.1;
Hatter 17:3c82070177e5 130 pwm[3] = 0.1;
Hatter 17:3c82070177e5 131
Hatter 15:1c1ae4077900 132 wait(count*10e-3); //count value * 0.01 sec
Hatter 15:1c1ae4077900 133 }
Hatter 18:9cbd70819d2d 134 //check = 0;
Hatter 15:1c1ae4077900 135 }
Hatter 15:1c1ae4077900 136
Hatter 0:fddd018033fb 137 int main()
Hatter 15:1c1ae4077900 138 {
Hatter 13:8a9d3855ab96 139
Hatter 13:8a9d3855ab96 140 /* PWM init */
Hatter 12:6e2ee1c83ac1 141 pwm[0] = PWM;
Hatter 12:6e2ee1c83ac1 142 pwm[1] = PWM;
Hatter 12:6e2ee1c83ac1 143 pwm[2] = PWM;
Hatter 12:6e2ee1c83ac1 144 pwm[3] = PWM;
Hatter 12:6e2ee1c83ac1 145
Hatter 10:5b67d18f30a9 146 xbee_packet *pt_packet=&packet;
Hatter 12:6e2ee1c83ac1 147
Hatter 17:3c82070177e5 148 //connect_check();
Hatter 17:3c82070177e5 149
Hatter 16:4b502070bea8 150 xbee.start_read();
Hatter 13:8a9d3855ab96 151 xbee.read_data(get_data,XBEE_KEY);
Hatter 13:8a9d3855ab96 152
Hatter 15:1c1ae4077900 153 int counter=0;
Hatter 12:6e2ee1c83ac1 154
Hatter 15:1c1ae4077900 155 for(;;) {
Hatter 15:1c1ae4077900 156
Hatter 15:1c1ae4077900 157 wait_ms(1.0);
Hatter 12:6e2ee1c83ac1 158
Hatter 13:8a9d3855ab96 159 //memcpy(&packet, get_data, DATA_NUM);
Hatter 10:5b67d18f30a9 160 packet.arm[0] = get_data[1];
Hatter 15:1c1ae4077900 161 packet.leg = get_data[0];
Hatter 12:6e2ee1c83ac1 162
Hatter 15:1c1ae4077900 163 //check = get_data[1];
Hatter 15:1c1ae4077900 164 armMbed.write_data(pt_packet->arm,ARM_KEY);
Hatter 12:6e2ee1c83ac1 165
Hatter 13:8a9d3855ab96 166 /* Stop */
Hatter 15:1c1ae4077900 167 if(packet.leg==0x0) {
Hatter 15:1c1ae4077900 168
Hatter 15:1c1ae4077900 169 counter /= 10e2; //mirisec -->> sec
Hatter 15:1c1ae4077900 170
Hatter 15:1c1ae4077900 171 if(counter>=TIME) { //wait 2 sec
Hatter 15:1c1ae4077900 172
Hatter 15:1c1ae4077900 173 stop_slowly(counter);
Hatter 15:1c1ae4077900 174 }
Hatter 13:8a9d3855ab96 175
Hatter 15:1c1ae4077900 176 motors = 0;
Hatter 15:1c1ae4077900 177 counter = 0; //cunter init
Hatter 15:1c1ae4077900 178
Hatter 15:1c1ae4077900 179 } else {
Hatter 15:1c1ae4077900 180
Hatter 15:1c1ae4077900 181 counter++;
Hatter 15:1c1ae4077900 182 }
Hatter 15:1c1ae4077900 183
Hatter 15:1c1ae4077900 184 /* PWM */
Hatter 15:1c1ae4077900 185 if((packet.leg>>4)==0) {
Hatter 15:1c1ae4077900 186
Hatter 14:731c52eb333b 187 pwm[0] = PWM_L;
Hatter 14:731c52eb333b 188 pwm[1] = PWM_R;
Hatter 14:731c52eb333b 189 pwm[2] = PWM_U;
Hatter 15:1c1ae4077900 190 pwm[3] = PWM_D;
Hatter 13:8a9d3855ab96 191
Hatter 15:1c1ae4077900 192 } else {
Hatter 14:731c52eb333b 193
Hatter 14:731c52eb333b 194 pwm[0] = PWM;
Hatter 14:731c52eb333b 195 pwm[1] = PWM;
Hatter 14:731c52eb333b 196 pwm[2] = PWM;
Hatter 14:731c52eb333b 197 pwm[3] = PWM;
Hatter 15:1c1ae4077900 198
Hatter 13:8a9d3855ab96 199 }
Hatter 12:6e2ee1c83ac1 200
Hatter 13:8a9d3855ab96 201 /* Direction of movement */
Hatter 15:1c1ae4077900 202 if(packet.leg&R) motors = 0x55; //01010101 R 接続に気をつけること
Hatter 15:1c1ae4077900 203 if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
Hatter 15:1c1ae4077900 204 if(packet.leg&U) motors = 0x5A; //01011010 U
Hatter 15:1c1ae4077900 205 if(packet.leg&D) motors = 0xA5; //10100101 D
Hatter 13:8a9d3855ab96 206
Hatter 13:8a9d3855ab96 207 /* Turn circling */
Hatter 16:4b502070bea8 208 if(packet.leg&L_t) motors = 0x66; //01100110
Hatter 16:4b502070bea8 209 if(packet.leg&R_t) motors = 0x99; //10011001
Hatter 13:8a9d3855ab96 210
Hatter 13:8a9d3855ab96 211 pc_print();
Hatter 6:26e5a520472d 212 }
Hatter 0:fddd018033fb 213 }