Bチーム / Mbed 2 deprecated receives_robot_wheel

受け取り足

Dependencies:   mbed serial_extend

main.cpp@4:1fc3f9627302, 2014-10-05 (annotated)

Committer:
bousiya03
Date:
Sun Oct 05 09:50:56 2014 +0000
Revision:
4:1fc3f9627302
Parent:
3:26ae0e32030d 
typo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bousiya03 0:f353f2d87281 1 /*
bousiya03 4:1fc3f9627302 2 * receives_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 3:26ae0e32030d 21 #define PWM 0.3
bousiya03 3:26ae0e32030d 22 #define PWM_SLOW 0.1
bousiya03 0:f353f2d87281 23
bousiya03 3:26ae0e32030d 24 #define R_R 0x4
bousiya03 3:26ae0e32030d 25 #define R_L 0x1
bousiya03 3:26ae0e32030d 26 #define R_U 0x2
bousiya03 3:26ae0e32030d 27 #define R_D 0x8
bousiya03 0:f353f2d87281 28
bousiya03 3:26ae0e32030d 29 #define L_R 0x10
bousiya03 3:26ae0e32030d 30 #define L_L 0x40
bousiya03 3:26ae0e32030d 31 #define L_U 0x20
bousiya03 3:26ae0e32030d 32 #define L_D 0x80
bousiya03 0:f353f2d87281 33
bousiya03 0:f353f2d87281 34 #define TIME 0
bousiya03 0:f353f2d87281 35
bousiya03 0:f353f2d87281 36 BusOut check(LED1,LED2,LED3,LED4);
bousiya03 0:f353f2d87281 37 BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20);
bousiya03 3:26ae0e32030d 38
bousiya03 0:f353f2d87281 39 PwmOut pwm[4]= {p21,p22,p23,p24};
bousiya03 0:f353f2d87281 40
bousiya03 0:f353f2d87281 41 Serial pc(USBTX,USBRX);
bousiya03 0:f353f2d87281 42
bousiya03 0:f353f2d87281 43 serial_extend xbee(p28,p27);
bousiya03 3:26ae0e32030d 44 serial_extend armMbed(p9,p10);
bousiya03 0:f353f2d87281 45
bousiya03 0:f353f2d87281 46 /* data structure */
bousiya03 0:f353f2d87281 47 typedef struct {
bousiya03 0:f353f2d87281 48
bousiya03 0:f353f2d87281 49 uint8_t arm[1];
bousiya03 0:f353f2d87281 50 uint8_t leg;
bousiya03 0:f353f2d87281 51
bousiya03 0:f353f2d87281 52 } xbee_packet;
bousiya03 0:f353f2d87281 53
bousiya03 0:f353f2d87281 54 xbee_packet packet;
bousiya03 0:f353f2d87281 55
bousiya03 0:f353f2d87281 56 uint8_t get_data[DATA_NUM]= {0};
bousiya03 0:f353f2d87281 57
bousiya03 3:26ae0e32030d 58 uint8_t check_data[2]={0};
bousiya03 0:f353f2d87281 59
bousiya03 3:26ae0e32030d 60 int counter=0;
bousiya03 0:f353f2d87281 61
bousiya03 0:f353f2d87281 62 int main()
bousiya03 0:f353f2d87281 63 {
bousiya03 0:f353f2d87281 64
bousiya03 3:26ae0e32030d 65 /* PWM init */
bousiya03 3:26ae0e32030d 66 pwm[0] = PWM;
bousiya03 3:26ae0e32030d 67 pwm[1] = PWM;
bousiya03 3:26ae0e32030d 68 pwm[2] = PWM;
bousiya03 3:26ae0e32030d 69 pwm[3] = PWM;
bousiya03 3:26ae0e32030d 70
bousiya03 0:f353f2d87281 71 xbee_packet *pt_packet=&packet;
bousiya03 0:f353f2d87281 72
bousiya03 3:26ae0e32030d 73 //connect_check();
bousiya03 3:26ae0e32030d 74
bousiya03 0:f353f2d87281 75 xbee.start_read();
bousiya03 0:f353f2d87281 76 xbee.read_data(get_data,XBEE_KEY);
bousiya03 3:26ae0e32030d 77
bousiya03 3:26ae0e32030d 78 armMbed.start_write();
bousiya03 3:26ae0e32030d 79 armMbed.write_data(pt_packet->arm,ARM_KEY);
bousiya03 2:7cd9f9cc2376 80
bousiya03 3:26ae0e32030d 81 counter=0;
bousiya03 3:26ae0e32030d 82
bousiya03 2:7cd9f9cc2376 83 check = 0xFF;
bousiya03 3:26ae0e32030d 84 wait(0.5);
bousiya03 0:f353f2d87281 85
bousiya03 0:f353f2d87281 86 for(;;) {
bousiya03 0:f353f2d87281 87
bousiya03 0:f353f2d87281 88 wait_ms(0.5);
bousiya03 0:f353f2d87281 89
bousiya03 3:26ae0e32030d 90 //memcpy(&packet, get_data, DATA_NUM);
bousiya03 0:f353f2d87281 91 packet.arm[0] = get_data[1];
bousiya03 0:f353f2d87281 92 packet.leg = get_data[0];
bousiya03 0:f353f2d87281 93
bousiya03 0:f353f2d87281 94 //check = get_data[1];
bousiya03 3:26ae0e32030d 95
bousiya03 0:f353f2d87281 96 check = packet.leg;
bousiya03 3:26ae0e32030d 97
bousiya03 0:f353f2d87281 98
bousiya03 0:f353f2d87281 99 /* Stop */
bousiya03 3:26ae0e32030d 100 if(packet.leg==0x0) {
bousiya03 3:26ae0e32030d 101
bousiya03 3:26ae0e32030d 102
bousiya03 3:26ae0e32030d 103 motors = 0;
bousiya03 3:26ae0e32030d 104
bousiya03 3:26ae0e32030d 105 pwm[0] = 0;
bousiya03 3:26ae0e32030d 106 pwm[1] = 0;
bousiya03 3:26ae0e32030d 107 pwm[2] = 0;
bousiya03 3:26ae0e32030d 108 pwm[3] = 0;
bousiya03 3:26ae0e32030d 109
bousiya03 3:26ae0e32030d 110
bousiya03 0:f353f2d87281 111
bousiya03 3:26ae0e32030d 112 }else{
bousiya03 3:26ae0e32030d 113
bousiya03 3:26ae0e32030d 114
bousiya03 3:26ae0e32030d 115 if((packet.leg==L_U) || (packet.leg==L_D)){
bousiya03 3:26ae0e32030d 116
bousiya03 3:26ae0e32030d 117 pwm[0] = PWM_SLOW;
bousiya03 3:26ae0e32030d 118 pwm[1] = PWM_SLOW;
bousiya03 3:26ae0e32030d 119 pwm[2] = PWM_SLOW;
bousiya03 3:26ae0e32030d 120 pwm[3] = PWM_SLOW;
bousiya03 3:26ae0e32030d 121
bousiya03 3:26ae0e32030d 122 }else{
bousiya03 3:26ae0e32030d 123
bousiya03 3:26ae0e32030d 124 pwm[0] = PWM;
bousiya03 3:26ae0e32030d 125 pwm[1] = PWM;
bousiya03 3:26ae0e32030d 126 pwm[2] = PWM;
bousiya03 3:26ae0e32030d 127 pwm[3] = PWM;
bousiya03 3:26ae0e32030d 128 }
bousiya03 3:26ae0e32030d 129 }
bousiya03 0:f353f2d87281 130
bousiya03 0:f353f2d87281 131 /* Direction of movement */
bousiya03 3:26ae0e32030d 132 if(packet.leg&R_D) motors = 0x55; //01010101 R 接続に気をつけること
bousiya03 3:26ae0e32030d 133 if(packet.leg&R_U) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
bousiya03 3:26ae0e32030d 134 if(packet.leg&R_R) motors = 0x5A; //01011010 U
bousiya03 3:26ae0e32030d 135 if(packet.leg&R_L) motors = 0xA5; //10100101 D
bousiya03 0:f353f2d87281 136
bousiya03 0:f353f2d87281 137 /* Turn circling */
bousiya03 3:26ae0e32030d 138 if(packet.leg&L_L) motors = 0x66; //01100110
bousiya03 3:26ae0e32030d 139 if(packet.leg&L_R) motors = 0x99; //10011001
bousiya03 3:26ae0e32030d 140 //if(packet.leg&L_U)
bousiya03 3:26ae0e32030d 141 //if(packet.leg&L_D)
bousiya03 3:26ae0e32030d 142
bousiya03 0:f353f2d87281 143 }
bousiya03 0:f353f2d87281 144 }