main.cpp

00001 //翼端can program
00002 #include "mbed.h"
00003 #include "ADXL345_I2C.h"
00004 #include "INA226.hpp"
00005 #define TO_SEND_DATAS_NUM 5
00006 #define INIT_SERVO_PERIOD_MS 20000
00007 #define WAIT_LOOP_TIME_MS 10
00008 #define CONTROL_VALUES_NUM 2
00009 #define TO_SEND_CAN_ID 100
00010 CAN can(p30,p29);
00011 CANMessage recmsg;
00012 Serial pc(USBTX,USBRX);
00013 ADXL345_I2C accelerometer(p9, p10);
00014 I2C ina226_i2c(p28,p27);
00015 INA226 VCmonitor(ina226_i2c);
00016 PwmOut servo1(p21);
00017 PwmOut servo2(p22);
00018 
00019 char toSendDatas[TO_SEND_DATAS_NUM];
00020 char controlValues[CONTROL_VALUES_NUM];
00021 
00022 int counter = 0;
00023 int eruron_deg = 0;
00024 int drug_deg = 0;
00025 unsigned short ina_val;
00026 double V,C;
00027 bool SERVO_FLAG;
00028 bool ADXL_FLAG;
00029 bool INA_FLAG;
00030 
00031 int acc[3] = {0,0,0};
00032 
00033 bool servoInit(){
00034     servo1.period_ms(INIT_SERVO_PERIOD_MS);
00035     servo2.period_ms(INIT_SERVO_PERIOD_MS);
00036     return true;
00037 }
00038 
00039 bool adxlInit(){
00040     accelerometer.setPowerControl(0x00);
00041     accelerometer.setDataFormatControl(0x0B);
00042     accelerometer.setDataRate(ADXL345_3200HZ);
00043     accelerometer.setPowerControl(0x08);
00044     return true;
00045 }
00046 
00047 bool inaInit(){
00048     if(!VCmonitor.isExist()){
00049         pc.printf("VCmonitor NOT FOUND\n");
00050         return false;
00051     }
00052     ina_val = 0;
00053     if(VCmonitor.rawRead(0x00,&ina_val) != 0){
00054         pc.printf("VCmonitor READ ERROR\n");
00055         return false;
00056     }
00057     VCmonitor.setCurrentCalibration();
00058     return true;
00059 }
00060 
00061 void init(){
00062     pc.printf("Receiver\n\r");
00063     SERVO_FLAG = servoInit();
00064     ADXL_FLAG = adxlInit();
00065     INA_FLAG = inaInit();
00066 }
00067 
00068 void updateDatas(){
00069     if(ADXL_FLAG){
00070         accelerometer.getOutput(acc);
00071     }
00072     if(INA_FLAG){
00073         int tmp = VCmonitor.getVoltage(&V);
00074         tmp = VCmonitor.getCurrent(&C);
00075     }
00076     for(int i = 0; i < 3; i++){
00077         toSendDatas[i] = acc[i];
00078     }
00079     toSendDatas[3] = (char)(V/100);
00080     toSendDatas[4] = (char)(C/0100);
00081 }
00082 
00083 void receiveDatas(){
00084     if(can.read(recmsg)){
00085         for(int i = 0; i < recmsg.len; i++){
00086             controlValues[i] = recmsg.data[i];
00087         }
00088     }
00089 }
00090 
00091 void toString(){
00092     //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){
00093         pc.printf("%i:",toSendDatas[3]);
00094         pc.printf("%i:",toSendDatas[4]);
00095         pc.printf("%f:",V);
00096         pc.printf("%f:",C);
00097     //}
00098     //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
00099 //        pc.printf("%i:",controlValues[i]);
00100 //    }
00101 //    pc.printf("%d",INA_FLAG);
00102     pc.printf("\n\r");
00103 }
00104 
00105 void sendDatas(){
00106     if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM)))
00107         pc.printf("resend suc\n\r");   
00108 }
00109 
00110 void 
00111 
00112 int main(){
00113     init();
00114     while(1){
00115         receiveDatas();
00116         updateDatas();
00117         //WriteServo();
00118         toString();
00119         sendDatas();
00120         wait_ms(WAIT_LOOP_TIME_MS);
00121     }
00122 }