main.cpp

00001 //相互通信確認用
00002 #include "mbed.h"
00003 #include "ADXL345_I2C.h"
00004 #define BUFFER 30
00005 
00006 Serial rs485(p13,p14);
00007 Serial pc(USBTX,USBRX);
00008 DigitalOut Receiver(p5);
00009 ADXL345_I2C accelerometer(p9, p10);
00010 PwmOut servo1(p21);
00011 PwmOut servo2(p22);
00012 
00013 int counter = 0;
00014 int servo_deg1 = 0;
00015 int servo_deg2 = 0;
00016 
00017 int acc[3] = {0,0,0};
00018 
00019 void rs485_rx()
00020 {
00021     signed char rec_data = rs485.getc();
00022     switch(rec_data) {
00023         case 'A':
00024             servo_deg1 = rs485.getc();
00025             pc.printf("counter1:%d\n\r",servo_deg1);
00026             break;
00027         case 'B':
00028             servo_deg2 = rs485.getc();
00029             //pc.printf("counter2:%d\n\r",servo_deg2);
00030             break;
00031         case 'C':
00032             Receiver = 1;
00033             wait_ms(1);
00034             rs485.putc('X');
00035             rs485.putc((signed char)acc[0]);
00036             rs485.putc('Y');
00037             rs485.putc((signed char)acc[1]);
00038             rs485.putc('Z');
00039             rs485.putc((signed char)acc[2]);
00040 //            rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]);
00041             wait_ms(2);
00042             Receiver = 0;
00043             //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
00044             break;
00045         default:
00046             wait_us(5);
00047             pc.printf("%d\n\r",rec_data);
00048     }
00049 }
00050 
00051 void init()
00052 {
00053     Receiver = 0;
00054     pc.printf("Receiver\n\r");
00055 
00056     rs485.baud(38400);
00057     rs485.attach(rs485_rx, Serial::RxIrq);
00058 
00059     servo1.period_ms(20);
00060     servo2.period_ms(20);
00061 
00062     accelerometer.setPowerControl(0x00);
00063     accelerometer.setDataFormatControl(0x0B);
00064     accelerometer.setDataRate(ADXL345_3200HZ);
00065     accelerometer.setPowerControl(0x08);
00066 }
00067 
00068 int main()
00069 {
00070     init();
00071     while(1) {
00072         accelerometer.getOutput(acc);
00073         servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
00074         servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
00075         wait_ms(10);
00076     }
00077 }