Diff: main.cpp

Revision:

3:2bdb60bbc665

Parent:

2:2b98a651b0a1

--- a/main.cpp	Sat Dec 05 11:21:06 2015 +0000
+++ b/main.cpp	Sat Jan 23 10:43:15 2016 +0000
@@ -1,18 +1,23 @@
-//相互通信確認用
+//翼端操舵プログラム
 #include "mbed.h"
 #include "ADXL345_I2C.h"
+#include "INA226.hpp"
 #define BUFFER 30
 
 Serial rs485(p13,p14);
 Serial pc(USBTX,USBRX);
 DigitalOut Receiver(p5);
 ADXL345_I2C accelerometer(p9, p10);
+I2C ina226_i2c(p28,p27);
+INA226 VCmonitor(ina226_i2c);
 PwmOut servo1(p21);
 PwmOut servo2(p22);
 
 int counter = 0;
-int servo_deg1 = 0;
-int servo_deg2 = 0;
+int eruron_deg = 0;
+int drug_deg = 0;
+unsigned short ina_val;
+double V,C;
 
 int acc[3] = {0,0,0};
 
@@ -21,12 +26,10 @@
     signed char rec_data = rs485.getc();
     switch(rec_data) {
         case 'A':
-            servo_deg1 = rs485.getc();
-            pc.printf("counter1:%d\n\r",servo_deg1);
+            eruron_deg = rs485.getc();
             break;
         case 'B':
-            servo_deg2 = rs485.getc();
-            //pc.printf("counter2:%d\n\r",servo_deg2);
+            drug_deg = rs485.getc();
             break;
         case 'C':
             Receiver = 1;
@@ -37,14 +40,15 @@
             rs485.putc((signed char)acc[1]);
             rs485.putc('Z');
             rs485.putc((signed char)acc[2]);
-//            rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]);
+            rs485.putc('V');
+            rs485.putc((signed char)V);
+            rs485.putc('I');
+            rs485.putc((signed char)C);
             wait_ms(2);
             Receiver = 0;
-            //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
             break;
         default:
             wait_us(5);
-            pc.printf("%d\n\r",rec_data);
     }
 }
 
@@ -63,15 +67,46 @@
     accelerometer.setDataFormatControl(0x0B);
     accelerometer.setDataRate(ADXL345_3200HZ);
     accelerometer.setPowerControl(0x08);
+
+    if(!VCmonitor.isExist()){
+        pc.printf("VCmonitor NOT FOUND\n");
+    }
+    ina_val = 0;
+    if(VCmonitor.rawRead(0x00,&ina_val) != 0){
+        pc.printf("VCmonitor READ ERROR\n");
+        while(1){}
+    }
+    VCmonitor.setCurrentCalibration();
+}
+
+void updateDatas(){
+    accelerometer.getOutput(acc);
+    int tmp = VCmonitor.getVoltage(&V);
+    tmp = VCmonitor.getCurrent(&C);
+}
+
+double calcPulse(int deg){
+    return (0.00093+(deg/180.0)*(0.00235-0.00077));
+}
+
+void WriteServo(){
+    servo1.pulsewidth(calcPulse(eruron_deg));
+    servo2.pulsewidth(calcPulse(drug_deg));    
+}
+
+void toStringDatas(){
+    pc.printf("acc[0]:%i   acc[1]:%i   acc[2]:%i\r\n",acc[0],acc[1],acc[2]);
+    pc.printf("V:%i   C:%i\r\n",V,C);
+    pc.printf("\r\n");
 }
 
 int main()
 {
     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));
+        updateDatas();
+        WriteServo();
+        toStringDatas();
         wait_ms(10);
     }
 }