Bチーム / Mbed 2 deprecated 4wd_omni_simple2

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

Dependencies:   mbed MultiSerial

main.cpp@13:8a9d3855ab96, 2014-09-08 (annotated)

Committer:
Hatter
Date:
Mon Sep 08 04:13:50 2014 +0000
Revision:
13:8a9d3855ab96
Parent:
12:6e2ee1c83ac1
Child:
14:731c52eb333b
??????; ??????????

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 9:f05f881226a3 18 #define DATA_NUM 8 //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 12:6e2ee1c83ac1 22 #define PWM 1.0
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 uint8_t value[4];
Hatter 13:8a9d3855ab96 51
Hatter 13:8a9d3855ab96 52 /* Out put PC stdout function */
Hatter 13:8a9d3855ab96 53 void pc_print(){
Hatter 13:8a9d3855ab96 54
Hatter 13:8a9d3855ab96 55 static int count=0;
Hatter 13:8a9d3855ab96 56
Hatter 13:8a9d3855ab96 57 if(count==10e3){
Hatter 13:8a9d3855ab96 58 pc.printf(" arm = %d" ,packet.arm[0]);
Hatter 13:8a9d3855ab96 59 pc.printf(" leg = %d" ,packet.leg);
Hatter 13:8a9d3855ab96 60 count=0;
Hatter 13:8a9d3855ab96 61 }
Hatter 13:8a9d3855ab96 62 count++;
Hatter 13:8a9d3855ab96 63 }
Hatter 0:fddd018033fb 64
Hatter 0:fddd018033fb 65 int main()
Hatter 13:8a9d3855ab96 66 {
Hatter 13:8a9d3855ab96 67
Hatter 13:8a9d3855ab96 68 /* PWM init */
Hatter 12:6e2ee1c83ac1 69 pwm[0] = PWM;
Hatter 12:6e2ee1c83ac1 70 pwm[1] = PWM;
Hatter 12:6e2ee1c83ac1 71 pwm[2] = PWM;
Hatter 12:6e2ee1c83ac1 72 pwm[3] = PWM;
Hatter 12:6e2ee1c83ac1 73
Hatter 13:8a9d3855ab96 74 value[0] = PWM_L;
Hatter 13:8a9d3855ab96 75 value[1] = PWM_R;
Hatter 13:8a9d3855ab96 76 value[2] = PWM_U;
Hatter 13:8a9d3855ab96 77 value[3] = PWM_D;
Hatter 13:8a9d3855ab96 78
Hatter 10:5b67d18f30a9 79 xbee_packet *pt_packet=&packet;
Hatter 12:6e2ee1c83ac1 80
Hatter 13:8a9d3855ab96 81 xbee.read_data(get_data,XBEE_KEY);
Hatter 13:8a9d3855ab96 82
Hatter 0:fddd018033fb 83 for(;;){
Hatter 12:6e2ee1c83ac1 84
Hatter 13:8a9d3855ab96 85 wait_ms(0.1);
Hatter 12:6e2ee1c83ac1 86
Hatter 13:8a9d3855ab96 87 //memcpy(&packet, get_data, DATA_NUM);
Hatter 9:f05f881226a3 88
Hatter 10:5b67d18f30a9 89 packet.arm[0] = get_data[1];
Hatter 10:5b67d18f30a9 90 packet.leg = get_data[0];
Hatter 12:6e2ee1c83ac1 91
Hatter 9:f05f881226a3 92 check = get_data[1];
Hatter 8:ee465babfd98 93
Hatter 12:6e2ee1c83ac1 94 armMbed.write_data(pt_packet->arm,ARM_KEY);
Hatter 12:6e2ee1c83ac1 95
Hatter 13:8a9d3855ab96 96 /* Stop */
Hatter 6:26e5a520472d 97 if(packet.leg==0x0) motors = 0;
Hatter 13:8a9d3855ab96 98
Hatter 13:8a9d3855ab96 99 /* PWM */
Hatter 13:8a9d3855ab96 100 if((packet.leg>>4)==0){
Hatter 13:8a9d3855ab96 101
Hatter 13:8a9d3855ab96 102 for(int i=0;i==4;i++){
Hatter 13:8a9d3855ab96 103 pwm[i] = value[i];
Hatter 13:8a9d3855ab96 104 }
Hatter 13:8a9d3855ab96 105 }
Hatter 12:6e2ee1c83ac1 106
Hatter 13:8a9d3855ab96 107 /* Direction of movement */
Hatter 13:8a9d3855ab96 108 if(packet.leg&0x1) motors = 0x55; //01010101
Hatter 13:8a9d3855ab96 109 if(packet.leg&0x8) motors = 0xAA; //10101010
Hatter 13:8a9d3855ab96 110 if(packet.leg&0x2) motors = 0x5A; //01011010
Hatter 13:8a9d3855ab96 111 if(packet.leg&0x4) motors = 0xA5; //10100101
Hatter 13:8a9d3855ab96 112
Hatter 13:8a9d3855ab96 113 /* Turn circling */
Hatter 13:8a9d3855ab96 114 if(packet.leg&0x10) motors = 0x66; //01010101
Hatter 13:8a9d3855ab96 115 if(packet.leg&0x80) motors = 0x99; //10101010
Hatter 13:8a9d3855ab96 116
Hatter 13:8a9d3855ab96 117 pc_print();
Hatter 13:8a9d3855ab96 118
Hatter 6:26e5a520472d 119 }
Hatter 0:fddd018033fb 120 }