Bチーム / Mbed 2 deprecated 4wd_omni_simple2

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

Dependencies:   mbed MultiSerial

main.cpp@16:4b502070bea8, 2014-09-23 (annotated)

Committer:
Hatter
Date:
Tue Sep 23 12:08:15 2014 +0000
Revision:
16:4b502070bea8
Parent:
15:1c1ae4077900
Child:
17:3c82070177e5
???????????

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 15:1c1ae4077900 22 #define PWM 1.0
Hatter 15:1c1ae4077900 23
Hatter 15:1c1ae4077900 24 #define PWM_L 1.0
Hatter 15:1c1ae4077900 25 #define PWM_R 1.0
Hatter 15:1c1ae4077900 26 #define PWM_U 1.0
Hatter 15:1c1ae4077900 27 #define PWM_D 1.0
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 16:4b502070bea8 61 uint8_t check_data[1];
Hatter 16:4b502070bea8 62
Hatter 13:8a9d3855ab96 63 /* Out put PC stdout function */
Hatter 15:1c1ae4077900 64
Hatter 13:8a9d3855ab96 65 void pc_print(){
Hatter 13:8a9d3855ab96 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 16:4b502070bea8 77 void connect_check(void){
Hatter 16:4b502070bea8 78 xbee.start_read();
Hatter 16:4b502070bea8 79 xbee.read_data(check_data,XBEE_KEY);
Hatter 16:4b502070bea8 80
Hatter 16:4b502070bea8 81 while(check_data[0]!=0xCC);
Hatter 16:4b502070bea8 82 while(check_data[0]==0xCC);
Hatter 16:4b502070bea8 83 xbee.stop_read();
Hatter 16:4b502070bea8 84 xbee.start_write();
Hatter 16:4b502070bea8 85 check_data[0]=0xCC;
Hatter 16:4b502070bea8 86 xbee.write_data(check_data,XBEE_KEY);
Hatter 16:4b502070bea8 87 wait_ms(10);
Hatter 16:4b502070bea8 88 xbee.stop_write();
Hatter 16:4b502070bea8 89 }
Hatter 16:4b502070bea8 90
Hatter 15:1c1ae4077900 91 void stop_slowly(int count)
Hatter 15:1c1ae4077900 92 {
Hatter 15:1c1ae4077900 93
Hatter 15:1c1ae4077900 94 for(float i=1.0; i>=0; i-=0.1) {
Hatter 15:1c1ae4077900 95
Hatter 15:1c1ae4077900 96 /*for(int j=0; j>=4; j++) {
Hatter 15:1c1ae4077900 97
Hatter 15:1c1ae4077900 98 pwm[j] = i;
Hatter 15:1c1ae4077900 99
Hatter 15:1c1ae4077900 100 }*/
Hatter 15:1c1ae4077900 101 pwm[0] = 0.1;
Hatter 15:1c1ae4077900 102 pwm[1] = 0.1;
Hatter 15:1c1ae4077900 103 pwm[2] = 0.1;
Hatter 15:1c1ae4077900 104 pwm[3] = 0.1;
Hatter 15:1c1ae4077900 105
Hatter 15:1c1ae4077900 106 wait(count*10e-3); //count value * 0.01 sec
Hatter 15:1c1ae4077900 107 }
Hatter 15:1c1ae4077900 108 check = 0;
Hatter 15:1c1ae4077900 109 }
Hatter 15:1c1ae4077900 110
Hatter 0:fddd018033fb 111 int main()
Hatter 15:1c1ae4077900 112 {
Hatter 13:8a9d3855ab96 113
Hatter 13:8a9d3855ab96 114 /* PWM init */
Hatter 12:6e2ee1c83ac1 115 pwm[0] = PWM;
Hatter 12:6e2ee1c83ac1 116 pwm[1] = PWM;
Hatter 12:6e2ee1c83ac1 117 pwm[2] = PWM;
Hatter 12:6e2ee1c83ac1 118 pwm[3] = PWM;
Hatter 12:6e2ee1c83ac1 119
Hatter 10:5b67d18f30a9 120 xbee_packet *pt_packet=&packet;
Hatter 12:6e2ee1c83ac1 121
Hatter 16:4b502070bea8 122 connect_check();
Hatter 16:4b502070bea8 123
Hatter 16:4b502070bea8 124 xbee.start_read();
Hatter 13:8a9d3855ab96 125 xbee.read_data(get_data,XBEE_KEY);
Hatter 13:8a9d3855ab96 126
Hatter 16:4b502070bea8 127 connect_check();
Hatter 16:4b502070bea8 128
Hatter 15:1c1ae4077900 129 int counter=0;
Hatter 12:6e2ee1c83ac1 130
Hatter 15:1c1ae4077900 131 for(;;) {
Hatter 15:1c1ae4077900 132
Hatter 15:1c1ae4077900 133 wait_ms(1.0);
Hatter 12:6e2ee1c83ac1 134
Hatter 13:8a9d3855ab96 135 //memcpy(&packet, get_data, DATA_NUM);
Hatter 9:f05f881226a3 136
Hatter 10:5b67d18f30a9 137 packet.arm[0] = get_data[1];
Hatter 15:1c1ae4077900 138 packet.leg = get_data[0];
Hatter 12:6e2ee1c83ac1 139
Hatter 15:1c1ae4077900 140 //check = get_data[1];
Hatter 8:ee465babfd98 141
Hatter 15:1c1ae4077900 142 armMbed.write_data(pt_packet->arm,ARM_KEY);
Hatter 12:6e2ee1c83ac1 143
Hatter 13:8a9d3855ab96 144 /* Stop */
Hatter 15:1c1ae4077900 145 if(packet.leg==0x0) {
Hatter 15:1c1ae4077900 146
Hatter 15:1c1ae4077900 147 counter /= 10e2; //mirisec -->> sec
Hatter 15:1c1ae4077900 148
Hatter 15:1c1ae4077900 149 if(counter>=TIME) { //wait 2 sec
Hatter 15:1c1ae4077900 150
Hatter 15:1c1ae4077900 151 stop_slowly(counter);
Hatter 15:1c1ae4077900 152 }
Hatter 13:8a9d3855ab96 153
Hatter 15:1c1ae4077900 154 motors = 0;
Hatter 15:1c1ae4077900 155 counter = 0; //cunter init
Hatter 15:1c1ae4077900 156
Hatter 15:1c1ae4077900 157 } else {
Hatter 15:1c1ae4077900 158
Hatter 15:1c1ae4077900 159 counter++;
Hatter 15:1c1ae4077900 160 check = 0xFF;
Hatter 15:1c1ae4077900 161 }
Hatter 15:1c1ae4077900 162
Hatter 15:1c1ae4077900 163 /* PWM */
Hatter 15:1c1ae4077900 164 if((packet.leg>>4)==0) {
Hatter 15:1c1ae4077900 165
Hatter 14:731c52eb333b 166 pwm[0] = PWM_L;
Hatter 14:731c52eb333b 167 pwm[1] = PWM_R;
Hatter 14:731c52eb333b 168 pwm[2] = PWM_U;
Hatter 15:1c1ae4077900 169 pwm[3] = PWM_D;
Hatter 13:8a9d3855ab96 170
Hatter 15:1c1ae4077900 171 } else {
Hatter 14:731c52eb333b 172
Hatter 14:731c52eb333b 173 pwm[0] = PWM;
Hatter 14:731c52eb333b 174 pwm[1] = PWM;
Hatter 14:731c52eb333b 175 pwm[2] = PWM;
Hatter 14:731c52eb333b 176 pwm[3] = PWM;
Hatter 15:1c1ae4077900 177
Hatter 13:8a9d3855ab96 178 }
Hatter 12:6e2ee1c83ac1 179
Hatter 13:8a9d3855ab96 180 /* Direction of movement */
Hatter 15:1c1ae4077900 181 if(packet.leg&R) motors = 0x55; //01010101 R 接続に気をつけること
Hatter 15:1c1ae4077900 182 if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
Hatter 15:1c1ae4077900 183 if(packet.leg&U) motors = 0x5A; //01011010 U
Hatter 15:1c1ae4077900 184 if(packet.leg&D) motors = 0xA5; //10100101 D
Hatter 13:8a9d3855ab96 185
Hatter 13:8a9d3855ab96 186 /* Turn circling */
Hatter 16:4b502070bea8 187 if(packet.leg&L_t) motors = 0x66; //01100110
Hatter 16:4b502070bea8 188 if(packet.leg&R_t) motors = 0x99; //10011001
Hatter 13:8a9d3855ab96 189
Hatter 13:8a9d3855ab96 190 pc_print();
Hatter 6:26e5a520472d 191 }
Hatter 0:fddd018033fb 192 }