Bチーム / Mbed 2 deprecated 4wd_omni_simple2

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

Dependencies:   mbed MultiSerial

main.cpp@24:37a17b7c6201, 2014-10-04 (annotated)

Committer:
bousiya03
Date:
Sat Oct 04 09:16:38 2014 +0000
Revision:
24:37a17b7c6201
Parent:
23:54fddee73c88 
fix some bugs

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 15:1c1ae4077900 41 PwmOut pwm[4]= {p21,p22,p23,p24};
Hatter 0:fddd018033fb 42
Hatter 9:f05f881226a3 43 Serial pc(USBTX,USBRX);
Hatter 0:fddd018033fb 44
Hatter 16:4b502070bea8 45 MultiSerial xbee(p28,p27);
bousiya03 24:37a17b7c6201 46 MultiSerial arm_mbed(p9,p10);
bousiya03 24:37a17b7c6201 47
bousiya03 24:37a17b7c6201 48
Hatter 0:fddd018033fb 49
Hatter 13:8a9d3855ab96 50 /* data structure */
Hatter 15:1c1ae4077900 51 typedef struct {
Hatter 6:26e5a520472d 52
Hatter 6:26e5a520472d 53 uint8_t arm[1];
Hatter 6:26e5a520472d 54 uint8_t leg;
Hatter 4:5da566692b79 55
Hatter 15:1c1ae4077900 56 } xbee_packet;
Hatter 6:26e5a520472d 57
Hatter 6:26e5a520472d 58 xbee_packet packet;
Hatter 6:26e5a520472d 59
Hatter 15:1c1ae4077900 60 uint8_t get_data[DATA_NUM]= {0};
Hatter 13:8a9d3855ab96 61
Hatter 18:9cbd70819d2d 62 uint8_t check_data[2]={0};
Hatter 18:9cbd70819d2d 63
Hatter 22:bfeb1c6e2539 64 int counter=0;
Hatter 22:bfeb1c6e2539 65
Hatter 16:4b502070bea8 66
Hatter 13:8a9d3855ab96 67 /* Out put PC stdout function */
Hatter 17:3c82070177e5 68 void pc_print()
Hatter 17:3c82070177e5 69 {
Hatter 13:8a9d3855ab96 70 static int count=0;
Hatter 13:8a9d3855ab96 71
Hatter 15:1c1ae4077900 72 if(count==10e3) {
Hatter 13:8a9d3855ab96 73 pc.printf(" arm = %d" ,packet.arm[0]);
Hatter 13:8a9d3855ab96 74 pc.printf(" leg = %d" ,packet.leg);
Hatter 13:8a9d3855ab96 75 count=0;
Hatter 15:1c1ae4077900 76 }
Hatter 13:8a9d3855ab96 77 count++;
Hatter 13:8a9d3855ab96 78 }
Hatter 0:fddd018033fb 79
Hatter 18:9cbd70819d2d 80 uint8_t readable_ratio(){
Hatter 18:9cbd70819d2d 81
Hatter 18:9cbd70819d2d 82 int check = 0;
Hatter 18:9cbd70819d2d 83
Hatter 18:9cbd70819d2d 84 for(int i=0;i>=100;i++){
Hatter 18:9cbd70819d2d 85
Hatter 18:9cbd70819d2d 86 if(xbee.readable_check()==1){check++;}
Hatter 18:9cbd70819d2d 87
bousiya03 24:37a17b7c6201 88 wait_ms(1.0);
Hatter 18:9cbd70819d2d 89 }
bousiya03 24:37a17b7c6201 90
Hatter 18:9cbd70819d2d 91 return check;
Hatter 18:9cbd70819d2d 92 }
Hatter 18:9cbd70819d2d 93
Hatter 17:3c82070177e5 94 void connect_check(void)
Hatter 17:3c82070177e5 95 {
Hatter 16:4b502070bea8 96 xbee.start_read();
Hatter 16:4b502070bea8 97 xbee.read_data(check_data,XBEE_KEY);
Hatter 18:9cbd70819d2d 98
Hatter 18:9cbd70819d2d 99 check = 0x01;
Hatter 18:9cbd70819d2d 100
Hatter 18:9cbd70819d2d 101 while(readable_ratio()>=50);
Hatter 18:9cbd70819d2d 102
Hatter 18:9cbd70819d2d 103 check = 0x03;
Hatter 18:9cbd70819d2d 104
Hatter 18:9cbd70819d2d 105 while(readable_ratio()<=50);
Hatter 17:3c82070177e5 106
Hatter 18:9cbd70819d2d 107 check = 0x07;
Hatter 18:9cbd70819d2d 108
Hatter 16:4b502070bea8 109 xbee.stop_read();
Hatter 16:4b502070bea8 110 xbee.start_write();
Hatter 17:3c82070177e5 111
Hatter 16:4b502070bea8 112 check_data[0]=0xCC;
Hatter 17:3c82070177e5 113
Hatter 16:4b502070bea8 114 xbee.write_data(check_data,XBEE_KEY);
Hatter 17:3c82070177e5 115
Hatter 16:4b502070bea8 116 wait_ms(10);
Hatter 17:3c82070177e5 117
Hatter 16:4b502070bea8 118 xbee.stop_write();
Hatter 17:3c82070177e5 119 }
Hatter 16:4b502070bea8 120
Hatter 22:bfeb1c6e2539 121 void move_slowly(int count)
Hatter 22:bfeb1c6e2539 122 {
Hatter 15:1c1ae4077900 123
Hatter 22:bfeb1c6e2539 124 float i=(count/10e3)*PWM; //mirisec -->> sec
Hatter 15:1c1ae4077900 125
Hatter 22:bfeb1c6e2539 126 pwm[0] = i;
Hatter 22:bfeb1c6e2539 127 pwm[1] = i;
Hatter 22:bfeb1c6e2539 128 pwm[2] = i;
Hatter 22:bfeb1c6e2539 129 pwm[3] = i;
Hatter 17:3c82070177e5 130
Hatter 23:54fddee73c88 131 //wait_ms(1.0); //count value * 0.01 sec
Hatter 22:bfeb1c6e2539 132
Hatter 18:9cbd70819d2d 133 //check = 0;
Hatter 15:1c1ae4077900 134 }
Hatter 15:1c1ae4077900 135
bousiya03 24:37a17b7c6201 136 void safety_mode()
bousiya03 24:37a17b7c6201 137 {
bousiya03 24:37a17b7c6201 138 xbee.stop_read();
bousiya03 24:37a17b7c6201 139 arm_mbed.stop_write();
bousiya03 24:37a17b7c6201 140
bousiya03 24:37a17b7c6201 141 for(;;) {
bousiya03 24:37a17b7c6201 142
bousiya03 24:37a17b7c6201 143 motors = 0;
bousiya03 24:37a17b7c6201 144
bousiya03 24:37a17b7c6201 145 check = 0xA;
bousiya03 24:37a17b7c6201 146 wait(0.5);
bousiya03 24:37a17b7c6201 147 check != 0xF;
bousiya03 24:37a17b7c6201 148 wait(0.5);
bousiya03 24:37a17b7c6201 149
bousiya03 24:37a17b7c6201 150 if(xbee.readable_check()==1) {
bousiya03 24:37a17b7c6201 151
bousiya03 24:37a17b7c6201 152 xbee.start_read();
bousiya03 24:37a17b7c6201 153 arm_mbed.start_write();
bousiya03 24:37a17b7c6201 154 return;
bousiya03 24:37a17b7c6201 155
bousiya03 24:37a17b7c6201 156 }
bousiya03 24:37a17b7c6201 157 }
bousiya03 24:37a17b7c6201 158 }
bousiya03 24:37a17b7c6201 159
Hatter 0:fddd018033fb 160 int main()
Hatter 15:1c1ae4077900 161 {
Hatter 13:8a9d3855ab96 162
bousiya03 24:37a17b7c6201 163
Hatter 12:6e2ee1c83ac1 164
Hatter 10:5b67d18f30a9 165 xbee_packet *pt_packet=&packet;
Hatter 12:6e2ee1c83ac1 166
Hatter 17:3c82070177e5 167 //connect_check();
Hatter 20:a322730ea5d7 168
Hatter 16:4b502070bea8 169 xbee.start_read();
Hatter 13:8a9d3855ab96 170 xbee.read_data(get_data,XBEE_KEY);
Hatter 22:bfeb1c6e2539 171
bousiya03 24:37a17b7c6201 172 arm_mbed.start_write();
bousiya03 24:37a17b7c6201 173 arm_mbed.write_data(pt_packet->arm,ARM_KEY);
Hatter 20:a322730ea5d7 174
Hatter 22:bfeb1c6e2539 175 counter=0;
bousiya03 24:37a17b7c6201 176
bousiya03 24:37a17b7c6201 177 int radio_check=0;
Hatter 12:6e2ee1c83ac1 178
Hatter 15:1c1ae4077900 179 for(;;) {
bousiya03 24:37a17b7c6201 180
bousiya03 24:37a17b7c6201 181 if(xbee.readable_check() == 0) {
bousiya03 24:37a17b7c6201 182
bousiya03 24:37a17b7c6201 183 radio_check++;
Hatter 15:1c1ae4077900 184
bousiya03 24:37a17b7c6201 185 if(radio_check>=500) {
bousiya03 24:37a17b7c6201 186 safety_mode();
bousiya03 24:37a17b7c6201 187 }
bousiya03 24:37a17b7c6201 188 }else{
bousiya03 24:37a17b7c6201 189
bousiya03 24:37a17b7c6201 190 radio_check = 0;
bousiya03 24:37a17b7c6201 191
bousiya03 24:37a17b7c6201 192 }
bousiya03 24:37a17b7c6201 193
Hatter 23:54fddee73c88 194 wait_ms(0.5);
Hatter 12:6e2ee1c83ac1 195
bousiya03 24:37a17b7c6201 196
Hatter 10:5b67d18f30a9 197 packet.arm[0] = get_data[1];
Hatter 15:1c1ae4077900 198 packet.leg = get_data[0];
Hatter 12:6e2ee1c83ac1 199
Hatter 23:54fddee73c88 200 //check = get_data[1];
Hatter 19:1305ce40f4e2 201
Hatter 23:54fddee73c88 202 check = 0xFF;
Hatter 23:54fddee73c88 203 if(packet.leg!=0){
Hatter 23:54fddee73c88 204
Hatter 23:54fddee73c88 205 check = packet.leg;
Hatter 23:54fddee73c88 206 }
Hatter 12:6e2ee1c83ac1 207
Hatter 13:8a9d3855ab96 208 /* Stop */
Hatter 22:bfeb1c6e2539 209 if(packet.leg==0x0) {
Hatter 23:54fddee73c88 210
Hatter 23:54fddee73c88 211 for(;counter>=0;counter--){
Hatter 22:bfeb1c6e2539 212
Hatter 23:54fddee73c88 213 //if(counter>=0){counter--;} //cunter init
Hatter 23:54fddee73c88 214
Hatter 23:54fddee73c88 215 move_slowly(counter);
Hatter 23:54fddee73c88 216 wait_ms(0.1);
Hatter 23:54fddee73c88 217
Hatter 23:54fddee73c88 218 }
Hatter 15:1c1ae4077900 219
Hatter 23:54fddee73c88 220 }else{
Hatter 15:1c1ae4077900 221
Hatter 23:54fddee73c88 222 if(counter<=10e3){
Hatter 23:54fddee73c88 223 counter++;
Hatter 23:54fddee73c88 224 }
Hatter 23:54fddee73c88 225
Hatter 23:54fddee73c88 226 move_slowly(counter);
Hatter 22:bfeb1c6e2539 227 }
Hatter 15:1c1ae4077900 228
Hatter 13:8a9d3855ab96 229 /* Direction of movement */
Hatter 15:1c1ae4077900 230 if(packet.leg&R) motors = 0x55; //01010101 R 接続に気をつけること
Hatter 15:1c1ae4077900 231 if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
Hatter 15:1c1ae4077900 232 if(packet.leg&U) motors = 0x5A; //01011010 U
Hatter 15:1c1ae4077900 233 if(packet.leg&D) motors = 0xA5; //10100101 D
Hatter 13:8a9d3855ab96 234
Hatter 13:8a9d3855ab96 235 /* Turn circling */
Hatter 22:bfeb1c6e2539 236 if(packet.leg&R_t) motors = 0x66; //01100110
Hatter 22:bfeb1c6e2539 237 if(packet.leg&L_t) motors = 0x99; //10011001
Hatter 13:8a9d3855ab96 238
Hatter 13:8a9d3855ab96 239 pc_print();
Hatter 6:26e5a520472d 240 }
Hatter 0:fddd018033fb 241 }