Bチーム / Mbed 2 deprecated receives_robot_wheel

受け取り足

Dependencies:   mbed serial_extend

main.cpp@2:7cd9f9cc2376, 2014-10-05 (annotated)

Committer:
bousiya03
Date:
Sun Oct 05 01:00:24 2014 +0000
Revision:
2:7cd9f9cc2376
Parent:
0:f353f2d87281
Child:
3:26ae0e32030d
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bousiya03 0:f353f2d87281 1 /*
bousiya03 0:f353f2d87281 2 * receibes_robot_wheel by Tomoki Hokida
bousiya03 0:f353f2d87281 3 *
bousiya03 0:f353f2d87281 4 * motor pins piar
bousiya03 0:f353f2d87281 5 * motor(p13,p14):motor(p15,p16)
bousiya03 0:f353f2d87281 6 * motor(p17,p18):motor(p19,p20)
bousiya03 0:f353f2d87281 7 *
bousiya03 0:f353f2d87281 8 * Serial Pins
bousiya03 0:f353f2d87281 9 * packet.arm[0]:arm
bousiya03 0:f353f2d87281 10 * packet.leg[1]:omni
bousiya03 0:f353f2d87281 11 * reset 0
bousiya03 0:f353f2d87281 12 */
bousiya03 0:f353f2d87281 13
bousiya03 0:f353f2d87281 14 #include "mbed.h"
bousiya03 0:f353f2d87281 15 #include "serial_extend.h"
bousiya03 0:f353f2d87281 16
bousiya03 0:f353f2d87281 17 #define DATA_NUM 2 //DATA_NUM[byte]通信 この値はmemcpy使ってるんでxbee_packet構造体のメモリ数といい感じに合わせるようにしてください
bousiya03 0:f353f2d87281 18 #define XBEE_KEY 0xAA //keycode
bousiya03 0:f353f2d87281 19 #define ARM_KEY 0xAA
bousiya03 0:f353f2d87281 20
bousiya03 0:f353f2d87281 21 #define PWM 0.5
bousiya03 0:f353f2d87281 22
bousiya03 0:f353f2d87281 23 #define R 0x4
bousiya03 0:f353f2d87281 24 #define L 0x1
bousiya03 0:f353f2d87281 25 #define U 0x2
bousiya03 0:f353f2d87281 26 #define D 0x8
bousiya03 0:f353f2d87281 27
bousiya03 0:f353f2d87281 28 #define R_t 0x10
bousiya03 0:f353f2d87281 29 #define L_t 0x40
bousiya03 0:f353f2d87281 30
bousiya03 0:f353f2d87281 31 #define TIME 0
bousiya03 0:f353f2d87281 32
bousiya03 0:f353f2d87281 33 BusOut check(LED1,LED2,LED3,LED4);
bousiya03 0:f353f2d87281 34 BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20);
bousiya03 0:f353f2d87281 35 PwmOut pwm[4]= {p21,p22,p23,p24};
bousiya03 0:f353f2d87281 36
bousiya03 0:f353f2d87281 37 Serial pc(USBTX,USBRX);
bousiya03 0:f353f2d87281 38
bousiya03 0:f353f2d87281 39 serial_extend xbee(p28,p27);
bousiya03 0:f353f2d87281 40 serial_extend arm_mbed(p9,p10);
bousiya03 0:f353f2d87281 41
bousiya03 0:f353f2d87281 42 /* data structure */
bousiya03 0:f353f2d87281 43 typedef struct {
bousiya03 0:f353f2d87281 44
bousiya03 0:f353f2d87281 45 uint8_t arm[1];
bousiya03 0:f353f2d87281 46 uint8_t leg;
bousiya03 0:f353f2d87281 47
bousiya03 0:f353f2d87281 48 } xbee_packet;
bousiya03 0:f353f2d87281 49
bousiya03 0:f353f2d87281 50 xbee_packet packet;
bousiya03 0:f353f2d87281 51
bousiya03 0:f353f2d87281 52 uint8_t get_data[DATA_NUM]= {0};
bousiya03 0:f353f2d87281 53 uint8_t check_data[2]= {0};
bousiya03 0:f353f2d87281 54
bousiya03 0:f353f2d87281 55 /* Out put PC stdout function */
bousiya03 0:f353f2d87281 56 void pc_print()
bousiya03 0:f353f2d87281 57 {
bousiya03 0:f353f2d87281 58 static int count=0;
bousiya03 0:f353f2d87281 59
bousiya03 0:f353f2d87281 60 if(count==10e3) {
bousiya03 0:f353f2d87281 61 pc.printf(" arm = %d" ,packet.arm[0]);
bousiya03 0:f353f2d87281 62 pc.printf(" leg = %d" ,packet.leg);
bousiya03 0:f353f2d87281 63 count=0;
bousiya03 0:f353f2d87281 64 }
bousiya03 0:f353f2d87281 65 count++;
bousiya03 0:f353f2d87281 66 }
bousiya03 0:f353f2d87281 67
bousiya03 2:7cd9f9cc2376 68 /*Safety Mode*/
bousiya03 0:f353f2d87281 69 void safety_mode()
bousiya03 0:f353f2d87281 70 {
bousiya03 0:f353f2d87281 71 xbee.stop_read();
bousiya03 0:f353f2d87281 72 arm_mbed.stop_write();
bousiya03 0:f353f2d87281 73
bousiya03 0:f353f2d87281 74 for(;;) {
bousiya03 0:f353f2d87281 75
bousiya03 0:f353f2d87281 76 motors = 0;
bousiya03 0:f353f2d87281 77
bousiya03 0:f353f2d87281 78 check = 0xA;
bousiya03 0:f353f2d87281 79 wait(0.5);
bousiya03 0:f353f2d87281 80 check != 0xF;
bousiya03 0:f353f2d87281 81 wait(0.5);
bousiya03 0:f353f2d87281 82
bousiya03 0:f353f2d87281 83 if(xbee.readable_check()==1) {
bousiya03 0:f353f2d87281 84
bousiya03 0:f353f2d87281 85 xbee.start_read();
bousiya03 0:f353f2d87281 86 arm_mbed.start_write();
bousiya03 0:f353f2d87281 87 return;
bousiya03 0:f353f2d87281 88
bousiya03 0:f353f2d87281 89 }
bousiya03 0:f353f2d87281 90 }
bousiya03 0:f353f2d87281 91 }
bousiya03 0:f353f2d87281 92
bousiya03 0:f353f2d87281 93 int main()
bousiya03 0:f353f2d87281 94 {
bousiya03 0:f353f2d87281 95
bousiya03 0:f353f2d87281 96 xbee_packet *pt_packet=&packet;
bousiya03 0:f353f2d87281 97
bousiya03 2:7cd9f9cc2376 98 /*Reception Start*/
bousiya03 0:f353f2d87281 99 xbee.start_read();
bousiya03 0:f353f2d87281 100 xbee.read_data(get_data,XBEE_KEY);
bousiya03 2:7cd9f9cc2376 101
bousiya03 2:7cd9f9cc2376 102 /*Send Start*/
bousiya03 0:f353f2d87281 103 arm_mbed.start_write();
bousiya03 0:f353f2d87281 104 arm_mbed.write_data(pt_packet->arm,ARM_KEY);
bousiya03 2:7cd9f9cc2376 105
bousiya03 2:7cd9f9cc2376 106 check = 0xFF;
bousiya03 0:f353f2d87281 107
bousiya03 0:f353f2d87281 108 //int radio_check=0;
bousiya03 0:f353f2d87281 109
bousiya03 0:f353f2d87281 110 for(;;) {
bousiya03 0:f353f2d87281 111
bousiya03 0:f353f2d87281 112 /*PWM init*/
bousiya03 0:f353f2d87281 113
bousiya03 0:f353f2d87281 114 pwm[0]=PWM;
bousiya03 0:f353f2d87281 115 pwm[1]=PWM;
bousiya03 0:f353f2d87281 116 pwm[2]=PWM;
bousiya03 0:f353f2d87281 117 pwm[3]=PWM;
bousiya03 0:f353f2d87281 118
bousiya03 0:f353f2d87281 119 /*
bousiya03 0:f353f2d87281 120 if(xbee.readable_check() == 0) {
bousiya03 0:f353f2d87281 121
bousiya03 0:f353f2d87281 122 radio_check++;
bousiya03 0:f353f2d87281 123
bousiya03 0:f353f2d87281 124 if(radio_check>=500){
bousiya03 0:f353f2d87281 125
bousiya03 0:f353f2d87281 126 safety_mode();
bousiya03 0:f353f2d87281 127 }
bousiya03 0:f353f2d87281 128
bousiya03 0:f353f2d87281 129 } else {
bousiya03 0:f353f2d87281 130
bousiya03 0:f353f2d87281 131 radio_check = 0;
bousiya03 0:f353f2d87281 132
bousiya03 0:f353f2d87281 133 }
bousiya03 0:f353f2d87281 134 */
bousiya03 2:7cd9f9cc2376 135
bousiya03 0:f353f2d87281 136 wait_ms(0.5);
bousiya03 0:f353f2d87281 137
bousiya03 0:f353f2d87281 138 packet.arm[0] = get_data[1];
bousiya03 0:f353f2d87281 139 packet.leg = get_data[0];
bousiya03 0:f353f2d87281 140
bousiya03 0:f353f2d87281 141 //check = get_data[1];
bousiya03 2:7cd9f9cc2376 142
bousiya03 0:f353f2d87281 143 check = packet.leg;
bousiya03 0:f353f2d87281 144
bousiya03 0:f353f2d87281 145
bousiya03 0:f353f2d87281 146 /* Stop */
bousiya03 0:f353f2d87281 147 if(packet.leg==0x0) {
bousiya03 0:f353f2d87281 148
bousiya03 0:f353f2d87281 149 motors = 0x00;
bousiya03 0:f353f2d87281 150 }
bousiya03 0:f353f2d87281 151
bousiya03 0:f353f2d87281 152 /* Direction of movement */
bousiya03 0:f353f2d87281 153 if(packet.leg&R) motors = 0x55; //01010101 R 接続に気をつけること
bousiya03 0:f353f2d87281 154 if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
bousiya03 0:f353f2d87281 155 if(packet.leg&U) motors = 0x5A; //01011010 U
bousiya03 0:f353f2d87281 156 if(packet.leg&D) motors = 0xA5; //10100101 D
bousiya03 0:f353f2d87281 157
bousiya03 0:f353f2d87281 158 /* Turn circling */
bousiya03 0:f353f2d87281 159 if(packet.leg&R_t) motors = 0x66; //01100110
bousiya03 0:f353f2d87281 160 if(packet.leg&L_t) motors = 0x99; //10011001
bousiya03 0:f353f2d87281 161
bousiya03 0:f353f2d87281 162 pc_print();
bousiya03 0:f353f2d87281 163 }
bousiya03 0:f353f2d87281 164 }