Bチーム / Mbed 2 deprecated receives_robot_wheel

受け取り足

Dependencies:   mbed serial_extend

main.cpp@3:26ae0e32030d, 2014-10-05 (annotated)

Committer:
bousiya03
Date:
Sun Oct 05 09:49:57 2014 +0000
Revision:
3:26ae0e32030d
Parent:
2:7cd9f9cc2376
Child:
4:1fc3f9627302
?????????????????????????

Who changed what in which revision?

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