操舵ソースコードの第一稿です。 右翼端です。
Dependencies: ADXL345_I2C INA226 mbed
main.cpp
- Committer:
- YusukeWakuta
- Date:
- 2016-01-20
- Revision:
- 1:ec22c326aa88
- Parent:
- 0:c7d5470d50e9
File content as of revision 1:ec22c326aa88:
#include "mbed.h" #include "ADXL345_I2C.h" #include "INA226.hpp" #define BUFFER 30 Serial rs485(p13,p14); DigitalOut Receiver(p5); ADXL345_I2C accelerometer(p9, p10); PwmOut servo1(p21); PwmOut servo2(p22); I2C i2c_Ina(p28,p27); INA226 Ina226(i2c_Ina); //ADXL345 accelerometer(p5, p6, p7, p8); Serial pc(USBTX, USBRX); void ADXL345(int *x) { // pc.printf("Starting ADXL345 test...\n"); //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId()); //Go into standby mode to configure the device. accelerometer.setPowerControl(0x00); //Full resolution, +/-16g, 4mg/LSB. accelerometer.setDataFormatControl(0x0B); //3.2kHz data rate. accelerometer.setDataRate(ADXL345_3200HZ); //Measurement mode. accelerometer.setPowerControl(0x08); accelerometer.getOutput(x); //13-bit, sign extended values. //pc.printf("%6i, %6i, %6i\n\r", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]); } int INA226_check(double *Voltage,double *Current) { while(1) { if(Ina226.isExist() == 0) { Ina226.getVoltage(Voltage); Ina226.getCurrent(Current); } //pc.printf("V : %lf, C : %lf \n\r",Voltage,Current); } return 0; } int counter = 0; int servo_deg1 = 0; int servo_deg2 = 0; int acc[3] = {0,0,0}; void rs485_rx() { int acc[3]; double *Voltage = 0; double *Current = 0; signed char rec_data = rs485.getc(); switch(rec_data) { case 'A': servo_deg1 = rs485.getc(); break; case 'B': servo_deg2 = rs485.getc(); break; case 'P': INA226_check(Voltage,Current); ADXL345(acc); Receiver = 1; wait_ms(3); rs485.putc('X'); rs485.putc((signed char)acc[0]); rs485.putc('Y'); rs485.putc((signed char)acc[1]); rs485.putc('Z'); rs485.putc((signed char)acc[2]); rs485.putc('V'); pc.printf("V"); rs485.putc((signed char)Voltage); pc.printf("%5d",(signed char)Voltage); rs485.putc('C'); pc.printf("C"); rs485.putc((signed char)Current); pc.printf("%5d\n\r",(signed char)Current); wait_ms(15); Receiver = 0; //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]); break; default: wait_us(5); } } void init() { Receiver = 0; pc.printf("Receiver\n\r"); rs485.baud(38400); rs485.attach(rs485_rx, Serial::RxIrq); servo1.period_ms(20); servo2.period_ms(20); accelerometer.setPowerControl(0x00); accelerometer.setDataFormatControl(0x0B); accelerometer.setDataRate(ADXL345_3200HZ); accelerometer.setPowerControl(0x08); } int main() { init(); while(1) { servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077)); servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077)); wait_ms(10); } }