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 PwmOut servo1(p21);
00012 PwmOut servo2(p22);
00013 I2C i2c_Ina(p28,p27);
00014 INA226 Ina226(i2c_Ina);
00015 
00016 
00017 int counter = 0;
00018 int servo_deg1 = 0;
00019 int servo_deg2 = 0;
00020 double *Voltage;
00021 double *Current;
00022 
00023 int acc[3] = {0,0,0};
00024 
00025 void rs485_rx()
00026 {
00027     signed char rec_data = rs485.getc();
00028     switch(rec_data) {
00029         case 'A':
00030             servo_deg1 = rs485.getc();
00031            // pc.printf("counter1:%d\n\r",servo_deg1);
00032             break;
00033         case 'B':
00034             servo_deg2 = rs485.getc();
00035             //pc.printf("counter2:%d\n\r",servo_deg2);
00036             break;
00037         case 'C':
00038         
00039     //counter++;
00040    // if(counter > 1000){
00041    // counter = 0;
00042    // }
00043             Receiver = 1;
00044             wait_ms(3);
00045            // if(counter % 2 == 0){
00046            rs485.putc('X');
00047           // pc.printf("X");
00048             rs485.putc((signed char)acc[0]);
00049             //pc.printf("%5d",(signed char)acc[0]);
00050             rs485.putc('Y');
00051             //pc.printf("Y");
00052             rs485.putc((signed char)acc[1]);
00053            // pc.printf("%5d",(signed char)acc[1]);
00054             rs485.putc('Z');
00055             //pc.printf("Z");
00056             rs485.putc((signed char)acc[2]);
00057             //}
00058            // else{
00059            // pc.printf("%5d",(signed char)acc[2]);
00060             rs485.putc('V');
00061            // pc.printf("V");
00062             rs485.putc((signed char)*Voltage);
00063           //  pc.printf("%5d",(signed char)*Voltage);
00064             rs485.putc('C');
00065             //pc.printf("C");
00066             rs485.putc((signed char)*Current);
00067             //pc.printf("%5d",(signed char)*Current);
00068             //}
00069             //pc.printf("\n\r");
00070 //            rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]);
00071             wait_ms(15);
00072             Receiver = 0;
00073             //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
00074             break;
00075         default:
00076             wait_us(5);
00077           //  pc.printf("%d\n\r",rec_data);
00078     }
00079 }
00080 
00081 void init()
00082 {
00083     Receiver = 0;
00084     pc.printf("Receiver\n\r");
00085 
00086     rs485.baud(38400);
00087     rs485.attach(rs485_rx, Serial::RxIrq);
00088 
00089     servo1.period_ms(20);
00090     servo2.period_ms(20);
00091 
00092     accelerometer.setPowerControl(0x00);
00093     accelerometer.setDataFormatControl(0x0B);
00094     accelerometer.setDataRate(ADXL345_3200HZ);
00095     accelerometer.setPowerControl(0x08);
00096 }
00097 
00098 void INA226(double *Voltage,double *Current){
00099     if(Ina226.isExist() == 0){
00100         Ina226.getVoltage(Voltage);
00101         Ina226.getCurrent(Current);
00102         }
00103     }
00104 
00105 int main()
00106 {
00107     init();
00108     while(1) {
00109         accelerometer.getOutput(acc);
00110         servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
00111         servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
00112         wait_ms(10);
00113     }
00114 }