Diff: main.cpp

Revision:

0:e052602db102

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jan 29 13:38:29 2016 +0000
@@ -0,0 +1,122 @@
+//翼端can program
+#include "mbed.h"
+#include "ADXL345_I2C.h"
+#include "INA226.hpp"
+#define TO_SEND_DATAS_NUM 5
+#define INIT_SERVO_PERIOD_MS 20000
+#define WAIT_LOOP_TIME_MS 10
+#define CONTROL_VALUES_NUM 2
+#define TO_SEND_CAN_ID 100
+CAN can(p30,p29);
+CANMessage recmsg;
+Serial pc(USBTX,USBRX);
+ADXL345_I2C accelerometer(p9, p10);
+I2C ina226_i2c(p28,p27);
+INA226 VCmonitor(ina226_i2c);
+PwmOut servo1(p21);
+PwmOut servo2(p22);
+
+char toSendDatas[TO_SEND_DATAS_NUM];
+char controlValues[CONTROL_VALUES_NUM];
+
+int counter = 0;
+int eruron_deg = 0;
+int drug_deg = 0;
+unsigned short ina_val;
+double V,C;
+bool SERVO_FLAG;
+bool ADXL_FLAG;
+bool INA_FLAG;
+
+int acc[3] = {0,0,0};
+
+bool servoInit(){
+    servo1.period_ms(INIT_SERVO_PERIOD_MS);
+    servo2.period_ms(INIT_SERVO_PERIOD_MS);
+    return true;
+}
+
+bool adxlInit(){
+    accelerometer.setPowerControl(0x00);
+    accelerometer.setDataFormatControl(0x0B);
+    accelerometer.setDataRate(ADXL345_3200HZ);
+    accelerometer.setPowerControl(0x08);
+    return true;
+}
+
+bool inaInit(){
+    if(!VCmonitor.isExist()){
+        pc.printf("VCmonitor NOT FOUND\n");
+        return false;
+    }
+    ina_val = 0;
+    if(VCmonitor.rawRead(0x00,&ina_val) != 0){
+        pc.printf("VCmonitor READ ERROR\n");
+        return false;
+    }
+    VCmonitor.setCurrentCalibration();
+    return true;
+}
+
+void init(){
+    pc.printf("Receiver\n\r");
+    SERVO_FLAG = servoInit();
+    ADXL_FLAG = adxlInit();
+    INA_FLAG = inaInit();
+}
+
+void updateDatas(){
+    if(ADXL_FLAG){
+        accelerometer.getOutput(acc);
+    }
+    if(INA_FLAG){
+        int tmp = VCmonitor.getVoltage(&V);
+        tmp = VCmonitor.getCurrent(&C);
+    }
+    for(int i = 0; i < 3; i++){
+        toSendDatas[i] = acc[i];
+    }
+    toSendDatas[3] = (char)(V/100);
+    toSendDatas[4] = (char)(C/0100);
+}
+
+void receiveDatas(){
+    if(can.read(recmsg)){
+        for(int i = 0; i < recmsg.len; i++){
+            controlValues[i] = recmsg.data[i];
+        }
+    }
+}
+
+void toString(){
+    //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){
+        pc.printf("%i:",toSendDatas[3]);
+        pc.printf("%i:",toSendDatas[4]);
+        pc.printf("%f:",V);
+        pc.printf("%f:",C);
+    //}
+    //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
+//        pc.printf("%i:",controlValues[i]);
+//    }
+//    pc.printf("%d",INA_FLAG);
+    pc.printf("\n\r");
+}
+
+void sendDatas(){
+    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM)))
+        pc.printf("resend suc\n\r");   
+}
+
+void 
+
+int main(){
+    init();
+    while(1){
+        receiveDatas();
+        updateDatas();
+        //WriteServo();
+        toString();
+        sendDatas();
+        wait_ms(WAIT_LOOP_TIME_MS);
+    }
+}
\ No newline at end of file