main.cpp

00001 //翼端操舵プログラム
00002 #include "mbed.h"
00003 #include "ADXL345_I2C.h"
00004 #include "INA226.hpp"
00005 #define BUFFER 30
00006 
00007 Serial rs485(p13,p14);
00008 Serial pc(USBTX,USBRX);
00009 DigitalOut Receiver(p5);
00010 ADXL345_I2C accelerometer(p9, p10);
00011 I2C ina226_i2c(p28,p27);
00012 INA226 VCmonitor(ina226_i2c);
00013 PwmOut servo1(p21);
00014 PwmOut servo2(p22);
00015 
00016 int counter = 0;
00017 int eruron_deg = 0;
00018 int drug_deg = 0;
00019 unsigned short ina_val;
00020 double V,C;
00021 
00022 int acc[3] = {0,0,0};
00023 
00024 void rs485_rx()
00025 {
00026     signed char rec_data = rs485.getc();
00027     switch(rec_data) {
00028         case 'A':
00029             eruron_deg = rs485.getc();
00030             break;
00031         case 'B':
00032             drug_deg = rs485.getc();
00033             break;
00034         case 'C':
00035             Receiver = 1;
00036             wait_ms(1);
00037             rs485.putc('X');
00038             rs485.putc((signed char)acc[0]);
00039             rs485.putc('Y');
00040             rs485.putc((signed char)acc[1]);
00041             rs485.putc('Z');
00042             rs485.putc((signed char)acc[2]);
00043             rs485.putc('V');
00044             rs485.putc((signed char)V);
00045             rs485.putc('I');
00046             rs485.putc((signed char)C);
00047             wait_ms(2);
00048             Receiver = 0;
00049             break;
00050         default:
00051             wait_us(5);
00052     }
00053 }
00054 
00055 void init()
00056 {
00057     Receiver = 0;
00058     pc.printf("Receiver\n\r");
00059 
00060     rs485.baud(38400);
00061     rs485.attach(rs485_rx, Serial::RxIrq);
00062 
00063     servo1.period_ms(20);
00064     servo2.period_ms(20);
00065 
00066     accelerometer.setPowerControl(0x00);
00067     accelerometer.setDataFormatControl(0x0B);
00068     accelerometer.setDataRate(ADXL345_3200HZ);
00069     accelerometer.setPowerControl(0x08);
00070 
00071     if(!VCmonitor.isExist()){
00072         pc.printf("VCmonitor NOT FOUND\n");
00073     }
00074     ina_val = 0;
00075     if(VCmonitor.rawRead(0x00,&ina_val) != 0){
00076         pc.printf("VCmonitor READ ERROR\n");
00077         while(1){}
00078     }
00079     VCmonitor.setCurrentCalibration();
00080 }
00081 
00082 void updateDatas(){
00083     accelerometer.getOutput(acc);
00084     int tmp = VCmonitor.getVoltage(&V);
00085     tmp = VCmonitor.getCurrent(&C);
00086 }
00087 
00088 double calcPulse(int deg){
00089     return (0.00093+(deg/180.0)*(0.00235-0.00077));
00090 }
00091 
00092 void WriteServo(){
00093     servo1.pulsewidth(calcPulse(eruron_deg));
00094     servo2.pulsewidth(calcPulse(drug_deg));    
00095 }
00096 
00097 void toStringDatas(){
00098     pc.printf("acc[0]:%i   acc[1]:%i   acc[2]:%i\r\n",acc[0],acc[1],acc[2]);
00099     pc.printf("V:%i   C:%i\r\n",V,C);
00100     pc.printf("\r\n");
00101 }
00102 
00103 int main()
00104 {
00105     init();
00106     while(1) {
00107         updateDatas();
00108         WriteServo();
00109         toStringDatas();
00110         wait_ms(10);
00111     }
00112 }