main.cpp

00001 #include "mbed.h"
00002 #include "ADXL345_I2C.h"
00003 #include "INA226.hpp"
00004 #define BUFFER 30
00005 
00006 
00007 Serial rs485(p13,p14);
00008 DigitalOut Receiver(p5);
00009 ADXL345_I2C accelerometer(p9, p10);
00010 PwmOut servo1(p21);
00011 PwmOut servo2(p22);
00012 I2C i2c_Ina(p28,p27);
00013 INA226 Ina226(i2c_Ina);
00014 
00015 Serial pc(USBTX, USBRX);
00016 
00017 void ADXL345(int *x)
00018 {
00019 
00020     // pc.printf("Starting ADXL345 test...\n");
00021     //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
00022 
00023     //Go into standby mode to configure the device.
00024     accelerometer.setPowerControl(0x00);
00025 
00026     //Full resolution, +/-16g, 4mg/LSB.
00027     accelerometer.setDataFormatControl(0x0B);
00028 
00029     //3.2kHz data rate.
00030     accelerometer.setDataRate(ADXL345_3200HZ);
00031 
00032     //Measurement mode.
00033     accelerometer.setPowerControl(0x08);
00034 
00035     accelerometer.getOutput(x);
00036 
00037     //13-bit, sign extended values.
00038     //pc.printf("%6i, %6i, %6i\n\r", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
00039 
00040 }
00041 
00042 int INA226_check(double *Voltage,double *Current)
00043 {
00044     while(1) {
00045         if(Ina226.isExist() == 0) {
00046             Ina226.getVoltage(Voltage);
00047             Ina226.getCurrent(Current);
00048         }
00049         //pc.printf("V : %lf, C : %lf \n\r",Voltage,Current);
00050 
00051     }
00052     return 0;
00053 }
00054 
00055 
00056 
00057 int counter = 0;
00058 int servo_deg1 = 0;
00059 int servo_deg2 = 0;
00060 
00061 int acc[3] = {0,0,0};
00062 
00063 void rs485_rx()
00064 {
00065     int acc[3];
00066     double *Voltage = 0;
00067     double *Current = 0;
00068     signed char rec_data = rs485.getc();
00069     switch(rec_data) {
00070         case 'C':
00071             servo_deg1 = rs485.getc();
00072             // pc.printf("counter1:%d\n\r",servo_deg1);
00073             break;
00074         case 'D':
00075             servo_deg2 = rs485.getc();
00076             //pc.printf("counter2:%d\n\r",servo_deg2);
00077             break;
00078         case 'P':
00079             INA226_check(Voltage,Current);
00080             ADXL345(acc);
00081             Receiver = 1;
00082             wait_ms(3);
00083             rs485.putc('X');
00084             pc.printf("X");
00085             rs485.putc((signed char)acc[0]);
00086             pc.printf("%5d",(signed char)acc[0]);
00087             rs485.putc('Y');
00088             pc.printf("Y");
00089             rs485.putc((signed char)acc[1]);
00090             pc.printf("%5d",(signed char)acc[1]);
00091             rs485.putc('Z');
00092             pc.printf("Z");
00093             pc.printf("%5d",(signed char)acc[2]);
00094             rs485.putc('V');
00095             pc.printf("V");
00096             rs485.putc((signed char)Voltage);
00097             pc.printf("%5d",(signed char)Voltage);
00098             rs485.putc('C');
00099             pc.printf("C");
00100             rs485.putc((signed char)Current);
00101             pc.printf("%5d\n\r",(signed char)Current);
00102 
00103             //pc.printf("\n\r");
00104 //            rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]);
00105             wait_ms(15);
00106             Receiver = 0;
00107             //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
00108             break;
00109         default:
00110             wait_us(5);
00111             //  pc.printf("%d\n\r",rec_data);
00112     }
00113 }
00114 
00115 void init()
00116 {
00117     Receiver = 0;
00118     pc.printf("Receiver\n\r");
00119 
00120     rs485.baud(38400);
00121     rs485.attach(rs485_rx, Serial::RxIrq);
00122 
00123     servo1.period_ms(20);
00124     servo2.period_ms(20);
00125 
00126     accelerometer.setPowerControl(0x00);
00127     accelerometer.setDataFormatControl(0x0B);
00128     accelerometer.setDataRate(ADXL345_3200HZ);
00129     accelerometer.setPowerControl(0x08);
00130 }
00131 
00132 
00133 int main()
00134 {
00135     init();
00136     while(1) {
00137         servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
00138         servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
00139         wait_ms(10);
00140     }
00141 }