Bチーム / Mbed 2 deprecated 4wd_omni_simple2

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

Dependencies:   mbed MultiSerial

main.cpp@14:731c52eb333b, 2014-09-08 (annotated)

Committer:
Hatter
Date:
Mon Sep 08 07:22:30 2014 +0000
Revision:
14:731c52eb333b
Parent:
13:8a9d3855ab96
Child:
15:1c1ae4077900
fix

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 13:8a9d3855ab96 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 14:731c52eb333b 22 #define PWM 0.5
Hatter 12:6e2ee1c83ac1 23
Hatter 13:8a9d3855ab96 24 #define PWM_L 0.5
Hatter 13:8a9d3855ab96 25 #define PWM_R 0.5
Hatter 13:8a9d3855ab96 26 #define PWM_U 0.5
Hatter 13:8a9d3855ab96 27 #define PWM_D 0.5
Hatter 13:8a9d3855ab96 28
Hatter 9:f05f881226a3 29 BusOut check(LED1,LED2,LED3,LED4);
Hatter 13:8a9d3855ab96 30 BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20);
Hatter 12:6e2ee1c83ac1 31
Hatter 12:6e2ee1c83ac1 32 PwmOut pwm[4]={p21,p22,p23,p24};
Hatter 0:fddd018033fb 33
Hatter 9:f05f881226a3 34 Serial pc(USBTX,USBRX);
Hatter 0:fddd018033fb 35
Hatter 12:6e2ee1c83ac1 36 MultiSerial xbee(p28,p27,read);
Hatter 13:8a9d3855ab96 37 MultiSerial armMbed(p9,p10,write);
Hatter 0:fddd018033fb 38
Hatter 13:8a9d3855ab96 39 /* data structure */
Hatter 6:26e5a520472d 40 typedef struct{
Hatter 6:26e5a520472d 41
Hatter 6:26e5a520472d 42 uint8_t arm[1];
Hatter 6:26e5a520472d 43 uint8_t leg;
Hatter 4:5da566692b79 44
Hatter 6:26e5a520472d 45 }xbee_packet;
Hatter 6:26e5a520472d 46
Hatter 6:26e5a520472d 47 xbee_packet packet;
Hatter 6:26e5a520472d 48
Hatter 10:5b67d18f30a9 49 uint8_t get_data[DATA_NUM]={0};
Hatter 13:8a9d3855ab96 50
Hatter 13:8a9d3855ab96 51 /* Out put PC stdout function */
Hatter 13:8a9d3855ab96 52 void pc_print(){
Hatter 13:8a9d3855ab96 53
Hatter 13:8a9d3855ab96 54 static int count=0;
Hatter 13:8a9d3855ab96 55
Hatter 13:8a9d3855ab96 56 if(count==10e3){
Hatter 13:8a9d3855ab96 57 pc.printf(" arm = %d" ,packet.arm[0]);
Hatter 13:8a9d3855ab96 58 pc.printf(" leg = %d" ,packet.leg);
Hatter 13:8a9d3855ab96 59 count=0;
Hatter 13:8a9d3855ab96 60 }
Hatter 13:8a9d3855ab96 61 count++;
Hatter 13:8a9d3855ab96 62 }
Hatter 0:fddd018033fb 63
Hatter 0:fddd018033fb 64 int main()
Hatter 13:8a9d3855ab96 65 {
Hatter 13:8a9d3855ab96 66
Hatter 13:8a9d3855ab96 67 /* PWM init */
Hatter 12:6e2ee1c83ac1 68 pwm[0] = PWM;
Hatter 12:6e2ee1c83ac1 69 pwm[1] = PWM;
Hatter 12:6e2ee1c83ac1 70 pwm[2] = PWM;
Hatter 12:6e2ee1c83ac1 71 pwm[3] = PWM;
Hatter 12:6e2ee1c83ac1 72
Hatter 10:5b67d18f30a9 73 xbee_packet *pt_packet=&packet;
Hatter 12:6e2ee1c83ac1 74
Hatter 13:8a9d3855ab96 75 xbee.read_data(get_data,XBEE_KEY);
Hatter 13:8a9d3855ab96 76
Hatter 0:fddd018033fb 77 for(;;){
Hatter 12:6e2ee1c83ac1 78
Hatter 13:8a9d3855ab96 79 wait_ms(0.1);
Hatter 12:6e2ee1c83ac1 80
Hatter 13:8a9d3855ab96 81 //memcpy(&packet, get_data, DATA_NUM);
Hatter 9:f05f881226a3 82
Hatter 10:5b67d18f30a9 83 packet.arm[0] = get_data[1];
Hatter 10:5b67d18f30a9 84 packet.leg = get_data[0];
Hatter 12:6e2ee1c83ac1 85
Hatter 9:f05f881226a3 86 check = get_data[1];
Hatter 8:ee465babfd98 87
Hatter 12:6e2ee1c83ac1 88 armMbed.write_data(pt_packet->arm,ARM_KEY);
Hatter 12:6e2ee1c83ac1 89
Hatter 13:8a9d3855ab96 90 /* Stop */
Hatter 6:26e5a520472d 91 if(packet.leg==0x0) motors = 0;
Hatter 13:8a9d3855ab96 92
Hatter 13:8a9d3855ab96 93 /* PWM */
Hatter 13:8a9d3855ab96 94 if((packet.leg>>4)==0){
Hatter 14:731c52eb333b 95
Hatter 14:731c52eb333b 96 pwm[0] = PWM_L;
Hatter 14:731c52eb333b 97 pwm[1] = PWM_R;
Hatter 14:731c52eb333b 98 pwm[2] = PWM_U;
Hatter 14:731c52eb333b 99 pwm[3] = PWM_D;
Hatter 13:8a9d3855ab96 100
Hatter 14:731c52eb333b 101 }else{
Hatter 14:731c52eb333b 102
Hatter 14:731c52eb333b 103 pwm[0] = PWM;
Hatter 14:731c52eb333b 104 pwm[1] = PWM;
Hatter 14:731c52eb333b 105 pwm[2] = PWM;
Hatter 14:731c52eb333b 106 pwm[3] = PWM;
Hatter 14:731c52eb333b 107
Hatter 13:8a9d3855ab96 108 }
Hatter 12:6e2ee1c83ac1 109
Hatter 13:8a9d3855ab96 110 /* Direction of movement */
Hatter 14:731c52eb333b 111 if(packet.leg&0x1) motors = 0x55; //01010101 L 接続に気をつけること
Hatter 14:731c52eb333b 112 if(packet.leg&0x4) motors = 0xAA; //10101010 U 多分モータードライバの信号線をそれぞれ逆にすればOKかな
Hatter 14:731c52eb333b 113 if(packet.leg&0x2) motors = 0x5A; //01011010 R
Hatter 14:731c52eb333b 114 if(packet.leg&0x8) motors = 0xA5; //10100101 D
Hatter 13:8a9d3855ab96 115
Hatter 13:8a9d3855ab96 116 /* Turn circling */
Hatter 14:731c52eb333b 117 if(packet.leg&0x10) motors = 0x66; //01100110
Hatter 14:731c52eb333b 118 if(packet.leg&0x40) motors = 0x99; //10011001
Hatter 13:8a9d3855ab96 119
Hatter 13:8a9d3855ab96 120 pc_print();
Hatter 13:8a9d3855ab96 121
Hatter 6:26e5a520472d 122 }
Hatter 0:fddd018033fb 123 }