操舵ソースコードの第一稿です。 右翼端です。

Dependencies:   ADXL345_I2C INA226 mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "ADXL345_I2C.h"
00003 #include "INA226.hpp"
00004 
00005 #define BUFFER 30
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 //ADXL345 accelerometer(p5, p6, p7, p8);
00016 Serial pc(USBTX, USBRX);
00017 
00018 void ADXL345(int *x)
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 'A':
00071             servo_deg1 = rs485.getc();
00072             break;
00073         case 'B':
00074             servo_deg2 = rs485.getc();
00075             break;
00076         case 'P':
00077             INA226_check(Voltage,Current);
00078             ADXL345(acc);
00079             Receiver = 1;
00080             wait_ms(3);
00081             rs485.putc('X');
00082             rs485.putc((signed char)acc[0]);
00083             rs485.putc('Y');
00084             rs485.putc((signed char)acc[1]);
00085             rs485.putc('Z');
00086             rs485.putc((signed char)acc[2]);
00087             rs485.putc('V');
00088             pc.printf("V");
00089             rs485.putc((signed char)Voltage);
00090             pc.printf("%5d",(signed char)Voltage);
00091             rs485.putc('C');
00092             pc.printf("C");
00093             rs485.putc((signed char)Current);
00094             pc.printf("%5d\n\r",(signed char)Current);
00095             wait_ms(15);
00096             Receiver = 0;
00097             //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
00098             break;
00099         default:
00100             wait_us(5);
00101     }
00102 }
00103 
00104 void init()
00105 {
00106     Receiver = 0;
00107     pc.printf("Receiver\n\r");
00108 
00109     rs485.baud(38400);
00110     rs485.attach(rs485_rx, Serial::RxIrq);
00111 
00112     servo1.period_ms(20);
00113     servo2.period_ms(20);
00114 
00115     accelerometer.setPowerControl(0x00);
00116     accelerometer.setDataFormatControl(0x0B);
00117     accelerometer.setDataRate(ADXL345_3200HZ);
00118     accelerometer.setPowerControl(0x08);
00119 }
00120 
00121 
00122 int main()
00123 {
00124     init();
00125     while(1) {
00126         servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
00127         servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
00128         wait_ms(10);
00129     }
00130 }