Bチーム / Mbed 2 deprecated 4wd_omni_simple2

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

Dependencies:   mbed MultiSerial

main.cpp@22:bfeb1c6e2539, 2014-10-01 (annotated)

Committer:
Hatter
Date:
Wed Oct 01 09:18:03 2014 +0000
Revision:
22:bfeb1c6e2539
Parent:
20:a322730ea5d7
Child:
23:54fddee73c88
add move_slowly()

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 22:bfeb1c6e2539 63 int counter=0;
Hatter 22:bfeb1c6e2539 64
Hatter 16:4b502070bea8 65
Hatter 13:8a9d3855ab96 66 /* Out put PC stdout function */
Hatter 17:3c82070177e5 67 void pc_print()
Hatter 17:3c82070177e5 68 {
Hatter 13:8a9d3855ab96 69 static int count=0;
Hatter 13:8a9d3855ab96 70
Hatter 15:1c1ae4077900 71 if(count==10e3) {
Hatter 13:8a9d3855ab96 72 pc.printf(" arm = %d" ,packet.arm[0]);
Hatter 13:8a9d3855ab96 73 pc.printf(" leg = %d" ,packet.leg);
Hatter 13:8a9d3855ab96 74 count=0;
Hatter 15:1c1ae4077900 75 }
Hatter 13:8a9d3855ab96 76 count++;
Hatter 13:8a9d3855ab96 77 }
Hatter 0:fddd018033fb 78
Hatter 18:9cbd70819d2d 79 uint8_t readable_ratio(){
Hatter 18:9cbd70819d2d 80
Hatter 18:9cbd70819d2d 81 int check = 0;
Hatter 18:9cbd70819d2d 82
Hatter 18:9cbd70819d2d 83 for(int i=0;i>=100;i++){
Hatter 18:9cbd70819d2d 84
Hatter 18:9cbd70819d2d 85 if(xbee.readable_check()==1){check++;}
Hatter 18:9cbd70819d2d 86
Hatter 18:9cbd70819d2d 87 wait_ms(1.0);
Hatter 18:9cbd70819d2d 88 }
Hatter 18:9cbd70819d2d 89 return check;
Hatter 18:9cbd70819d2d 90 }
Hatter 18:9cbd70819d2d 91
Hatter 17:3c82070177e5 92 void connect_check(void)
Hatter 17:3c82070177e5 93 {
Hatter 16:4b502070bea8 94 xbee.start_read();
Hatter 16:4b502070bea8 95 xbee.read_data(check_data,XBEE_KEY);
Hatter 18:9cbd70819d2d 96
Hatter 18:9cbd70819d2d 97 check = 0x01;
Hatter 18:9cbd70819d2d 98
Hatter 18:9cbd70819d2d 99 while(readable_ratio()>=50);
Hatter 18:9cbd70819d2d 100
Hatter 18:9cbd70819d2d 101 check = 0x03;
Hatter 18:9cbd70819d2d 102
Hatter 18:9cbd70819d2d 103 while(readable_ratio()<=50);
Hatter 17:3c82070177e5 104
Hatter 18:9cbd70819d2d 105 check = 0x07;
Hatter 18:9cbd70819d2d 106
Hatter 16:4b502070bea8 107 xbee.stop_read();
Hatter 16:4b502070bea8 108 xbee.start_write();
Hatter 17:3c82070177e5 109
Hatter 16:4b502070bea8 110 check_data[0]=0xCC;
Hatter 17:3c82070177e5 111
Hatter 16:4b502070bea8 112 xbee.write_data(check_data,XBEE_KEY);
Hatter 17:3c82070177e5 113
Hatter 16:4b502070bea8 114 wait_ms(10);
Hatter 17:3c82070177e5 115
Hatter 16:4b502070bea8 116 xbee.stop_write();
Hatter 17:3c82070177e5 117 }
Hatter 16:4b502070bea8 118
Hatter 22:bfeb1c6e2539 119 void move_slowly(int count)
Hatter 22:bfeb1c6e2539 120 {
Hatter 15:1c1ae4077900 121
Hatter 22:bfeb1c6e2539 122 float i=(count/10e3)*PWM; //mirisec -->> sec
Hatter 15:1c1ae4077900 123
Hatter 22:bfeb1c6e2539 124 pwm[0] = i;
Hatter 22:bfeb1c6e2539 125 pwm[1] = i;
Hatter 22:bfeb1c6e2539 126 pwm[2] = i;
Hatter 22:bfeb1c6e2539 127 pwm[3] = i;
Hatter 17:3c82070177e5 128
Hatter 22:bfeb1c6e2539 129 wait(count*2*10e-3); //count value * 0.01 sec
Hatter 22:bfeb1c6e2539 130
Hatter 18:9cbd70819d2d 131 //check = 0;
Hatter 15:1c1ae4077900 132 }
Hatter 15:1c1ae4077900 133
Hatter 0:fddd018033fb 134 int main()
Hatter 15:1c1ae4077900 135 {
Hatter 13:8a9d3855ab96 136
Hatter 13:8a9d3855ab96 137 /* PWM init */
Hatter 12:6e2ee1c83ac1 138 pwm[0] = PWM;
Hatter 12:6e2ee1c83ac1 139 pwm[1] = PWM;
Hatter 12:6e2ee1c83ac1 140 pwm[2] = PWM;
Hatter 12:6e2ee1c83ac1 141 pwm[3] = PWM;
Hatter 12:6e2ee1c83ac1 142
Hatter 10:5b67d18f30a9 143 xbee_packet *pt_packet=&packet;
Hatter 12:6e2ee1c83ac1 144
Hatter 17:3c82070177e5 145 //connect_check();
Hatter 20:a322730ea5d7 146
Hatter 16:4b502070bea8 147 xbee.start_read();
Hatter 13:8a9d3855ab96 148 xbee.read_data(get_data,XBEE_KEY);
Hatter 22:bfeb1c6e2539 149
Hatter 20:a322730ea5d7 150 armMbed.start_write();
Hatter 20:a322730ea5d7 151 armMbed.write_data(pt_packet->arm,ARM_KEY);
Hatter 20:a322730ea5d7 152
Hatter 22:bfeb1c6e2539 153 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 19:1305ce40f4e2 163 check = get_data[1];
Hatter 19:1305ce40f4e2 164
Hatter 12:6e2ee1c83ac1 165
Hatter 13:8a9d3855ab96 166 /* Stop */
Hatter 22:bfeb1c6e2539 167 if(packet.leg==0x0) {
Hatter 15:1c1ae4077900 168
Hatter 22:bfeb1c6e2539 169 if(counter!=0){
Hatter 22:bfeb1c6e2539 170
Hatter 22:bfeb1c6e2539 171 move_slowly(counter);
Hatter 22:bfeb1c6e2539 172
Hatter 22:bfeb1c6e2539 173 counter--;
Hatter 22:bfeb1c6e2539 174 } //cunter init
Hatter 15:1c1ae4077900 175
Hatter 13:8a9d3855ab96 176
Hatter 15:1c1ae4077900 177 motors = 0;
Hatter 22:bfeb1c6e2539 178
Hatter 15:1c1ae4077900 179
Hatter 15:1c1ae4077900 180 } else {
Hatter 15:1c1ae4077900 181
Hatter 22:bfeb1c6e2539 182 if(counter<=10e3){counter++;}
Hatter 22:bfeb1c6e2539 183 }
Hatter 15:1c1ae4077900 184
Hatter 15:1c1ae4077900 185 /* PWM */
Hatter 15:1c1ae4077900 186 if((packet.leg>>4)==0) {
Hatter 15:1c1ae4077900 187
Hatter 14:731c52eb333b 188 pwm[0] = PWM_L;
Hatter 14:731c52eb333b 189 pwm[1] = PWM_R;
Hatter 14:731c52eb333b 190 pwm[2] = PWM_U;
Hatter 15:1c1ae4077900 191 pwm[3] = PWM_D;
Hatter 13:8a9d3855ab96 192
Hatter 15:1c1ae4077900 193 } else {
Hatter 14:731c52eb333b 194
Hatter 14:731c52eb333b 195 pwm[0] = PWM;
Hatter 14:731c52eb333b 196 pwm[1] = PWM;
Hatter 14:731c52eb333b 197 pwm[2] = PWM;
Hatter 14:731c52eb333b 198 pwm[3] = PWM;
Hatter 22:bfeb1c6e2539 199
Hatter 13:8a9d3855ab96 200 }
Hatter 12:6e2ee1c83ac1 201
Hatter 13:8a9d3855ab96 202 /* Direction of movement */
Hatter 15:1c1ae4077900 203 if(packet.leg&R) motors = 0x55; //01010101 R 接続に気をつけること
Hatter 15:1c1ae4077900 204 if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
Hatter 15:1c1ae4077900 205 if(packet.leg&U) motors = 0x5A; //01011010 U
Hatter 15:1c1ae4077900 206 if(packet.leg&D) motors = 0xA5; //10100101 D
Hatter 13:8a9d3855ab96 207
Hatter 13:8a9d3855ab96 208 /* Turn circling */
Hatter 22:bfeb1c6e2539 209 if(packet.leg&R_t) motors = 0x66; //01100110
Hatter 22:bfeb1c6e2539 210 if(packet.leg&L_t) motors = 0x99; //10011001
Hatter 13:8a9d3855ab96 211
Hatter 13:8a9d3855ab96 212 pc_print();
Hatter 6:26e5a520472d 213 }
Hatter 0:fddd018033fb 214 }