Diff: main.cpp

Revision:

1:563b7d0b896f

Parent:

0:fafac0045ad3

Child:

2:2b98a651b0a1

diff -r fafac0045ad3 -r 563b7d0b896f main.cpp
--- a/main.cpp	Fri Dec 04 22:42:20 2015 +0000
+++ b/main.cpp	Sat Dec 05 03:19:30 2015 +0000
@@ -1,10 +1,12 @@
 //相互通信確認用
 #include "mbed.h"
+#include "ADXL345_I2C.h"
 #define BUFFER 30
 
-Serial rs485(p9,p10);
+Serial rs485(p13,p14);
 Serial pc(USBTX,USBRX);
 DigitalOut Receiver(p5);
+ADXL345_I2C accelerometer(p9, p10);
 PwmOut servo1(p21);
 PwmOut servo2(p22);
 
@@ -12,41 +14,62 @@
 int servo_deg1 = 0;
 int servo_deg2 = 0;
 
+int acc[3] = {0,0,0};
+
 void rs485_rx()
 {
     int rec_data = rs485.getc();
-    switch(rec_data){
-    case 'A':    
-        servo_deg1 = rs485.getc();
-        pc.printf("counter1:%d\n\r",servo_deg1);
-        break;
-    case 'B':
-        servo_deg2 = rs485.getc();
-        pc.printf("counter2:%d\n\r",servo_deg2);
-        break;
-    case 'C':
-        Receiver = 1;
-        wait_ms(1);
-        rs485.putc(counter++);
-        wait_ms(1);
-        Receiver = 0;
-        break;
-    default:
-        wait_us(5);
-        pc.printf("%d\n\r",rec_data);
+    switch(rec_data) {
+        case 'A':
+            servo_deg1 = rs485.getc();
+            pc.printf("counter1:%d\n\r",servo_deg1);
+            break;
+        case 'B':
+            servo_deg2 = rs485.getc();
+            pc.printf("counter2:%d\n\r",servo_deg2);
+            break;
+        case 'C':
+            Receiver = 1;
+            wait_ms(1);
+            rs485.putc('X');
+            rs485.putc(acc[0]);
+            rs485.putc('Y');
+            rs485.putc(acc[1]);
+            rs485.putc('Z');
+            rs485.putc(acc[2]);
+            wait_ms(2);
+            Receiver = 0;
+            break;
+        default:
+            wait_us(5);
+            pc.printf("%d\n\r",rec_data);
     }
 }
 
+void init()
+{
+    Receiver = 0;
+    pc.printf("Receiver\n\r");
+
+    rs485.baud(38400);
+    rs485.attach(rs485_rx, Serial::RxIrq);
+
+    servo1.period_ms(20);
+    servo2.period_ms(20);
+
+    accelerometer.setPowerControl(0x00);
+    accelerometer.setDataFormatControl(0x0B);
+    accelerometer.setDataRate(ADXL345_3200HZ);
+    accelerometer.setPowerControl(0x08);
+}
+
 int main()
 {
-    Receiver = 0;
-    pc.printf("Receiver\n\r");
-    rs485.baud(38400);
-    servo1.period_ms(20);
-    servo2.period_ms(20);
-    rs485.attach(rs485_rx, Serial::RxIrq);
-    while(1){
+    init();
+    while(1) {
+        accelerometer.getOutput(acc);
         servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
         servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
+        wait_ms(100);
     }
 }