操舵ソースコードの第一稿です。 右翼端です。
Dependencies: ADXL345_I2C INA226 mbed
main.cpp@1:ec22c326aa88, 2016-01-20 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Wed Jan 20 13:42:58 2016 +0000
- Revision:
- 1:ec22c326aa88
- Parent:
- 0:c7d5470d50e9
1?20?????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
YusukeWakuta | 0:c7d5470d50e9 | 1 | #include "mbed.h" |
YusukeWakuta | 0:c7d5470d50e9 | 2 | #include "ADXL345_I2C.h" |
YusukeWakuta | 0:c7d5470d50e9 | 3 | #include "INA226.hpp" |
YusukeWakuta | 1:ec22c326aa88 | 4 | |
YusukeWakuta | 0:c7d5470d50e9 | 5 | #define BUFFER 30 |
YusukeWakuta | 0:c7d5470d50e9 | 6 | |
YusukeWakuta | 0:c7d5470d50e9 | 7 | Serial rs485(p13,p14); |
YusukeWakuta | 0:c7d5470d50e9 | 8 | DigitalOut Receiver(p5); |
YusukeWakuta | 0:c7d5470d50e9 | 9 | ADXL345_I2C accelerometer(p9, p10); |
YusukeWakuta | 0:c7d5470d50e9 | 10 | PwmOut servo1(p21); |
YusukeWakuta | 0:c7d5470d50e9 | 11 | PwmOut servo2(p22); |
YusukeWakuta | 0:c7d5470d50e9 | 12 | I2C i2c_Ina(p28,p27); |
YusukeWakuta | 0:c7d5470d50e9 | 13 | INA226 Ina226(i2c_Ina); |
YusukeWakuta | 1:ec22c326aa88 | 14 | |
YusukeWakuta | 0:c7d5470d50e9 | 15 | //ADXL345 accelerometer(p5, p6, p7, p8); |
YusukeWakuta | 0:c7d5470d50e9 | 16 | Serial pc(USBTX, USBRX); |
YusukeWakuta | 1:ec22c326aa88 | 17 | |
YusukeWakuta | 0:c7d5470d50e9 | 18 | void ADXL345(int *x) |
YusukeWakuta | 0:c7d5470d50e9 | 19 | { |
YusukeWakuta | 1:ec22c326aa88 | 20 | // pc.printf("Starting ADXL345 test...\n"); |
YusukeWakuta | 0:c7d5470d50e9 | 21 | //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId()); |
YusukeWakuta | 1:ec22c326aa88 | 22 | |
YusukeWakuta | 0:c7d5470d50e9 | 23 | //Go into standby mode to configure the device. |
YusukeWakuta | 0:c7d5470d50e9 | 24 | accelerometer.setPowerControl(0x00); |
YusukeWakuta | 1:ec22c326aa88 | 25 | |
YusukeWakuta | 0:c7d5470d50e9 | 26 | //Full resolution, +/-16g, 4mg/LSB. |
YusukeWakuta | 0:c7d5470d50e9 | 27 | accelerometer.setDataFormatControl(0x0B); |
YusukeWakuta | 1:ec22c326aa88 | 28 | |
YusukeWakuta | 0:c7d5470d50e9 | 29 | //3.2kHz data rate. |
YusukeWakuta | 0:c7d5470d50e9 | 30 | accelerometer.setDataRate(ADXL345_3200HZ); |
YusukeWakuta | 1:ec22c326aa88 | 31 | |
YusukeWakuta | 0:c7d5470d50e9 | 32 | //Measurement mode. |
YusukeWakuta | 0:c7d5470d50e9 | 33 | accelerometer.setPowerControl(0x08); |
YusukeWakuta | 0:c7d5470d50e9 | 34 | |
YusukeWakuta | 1:ec22c326aa88 | 35 | accelerometer.getOutput(x); |
YusukeWakuta | 1:ec22c326aa88 | 36 | |
YusukeWakuta | 1:ec22c326aa88 | 37 | //13-bit, sign extended values. |
YusukeWakuta | 1:ec22c326aa88 | 38 | //pc.printf("%6i, %6i, %6i\n\r", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]); |
YusukeWakuta | 1:ec22c326aa88 | 39 | |
YusukeWakuta | 0:c7d5470d50e9 | 40 | } |
YusukeWakuta | 0:c7d5470d50e9 | 41 | |
YusukeWakuta | 1:ec22c326aa88 | 42 | int INA226_check(double *Voltage,double *Current) |
YusukeWakuta | 1:ec22c326aa88 | 43 | { |
YusukeWakuta | 1:ec22c326aa88 | 44 | while(1) { |
YusukeWakuta | 1:ec22c326aa88 | 45 | if(Ina226.isExist() == 0) { |
YusukeWakuta | 1:ec22c326aa88 | 46 | Ina226.getVoltage(Voltage); |
YusukeWakuta | 1:ec22c326aa88 | 47 | Ina226.getCurrent(Current); |
YusukeWakuta | 0:c7d5470d50e9 | 48 | } |
YusukeWakuta | 1:ec22c326aa88 | 49 | //pc.printf("V : %lf, C : %lf \n\r",Voltage,Current); |
YusukeWakuta | 1:ec22c326aa88 | 50 | |
YusukeWakuta | 0:c7d5470d50e9 | 51 | } |
YusukeWakuta | 0:c7d5470d50e9 | 52 | return 0; |
YusukeWakuta | 1:ec22c326aa88 | 53 | } |
YusukeWakuta | 0:c7d5470d50e9 | 54 | |
YusukeWakuta | 0:c7d5470d50e9 | 55 | |
YusukeWakuta | 0:c7d5470d50e9 | 56 | |
YusukeWakuta | 0:c7d5470d50e9 | 57 | int counter = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 58 | int servo_deg1 = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 59 | int servo_deg2 = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 60 | |
YusukeWakuta | 0:c7d5470d50e9 | 61 | int acc[3] = {0,0,0}; |
YusukeWakuta | 0:c7d5470d50e9 | 62 | |
YusukeWakuta | 0:c7d5470d50e9 | 63 | void rs485_rx() |
YusukeWakuta | 0:c7d5470d50e9 | 64 | { |
YusukeWakuta | 0:c7d5470d50e9 | 65 | int acc[3]; |
YusukeWakuta | 1:ec22c326aa88 | 66 | double *Voltage = 0; |
YusukeWakuta | 1:ec22c326aa88 | 67 | double *Current = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 68 | signed char rec_data = rs485.getc(); |
YusukeWakuta | 0:c7d5470d50e9 | 69 | switch(rec_data) { |
YusukeWakuta | 0:c7d5470d50e9 | 70 | case 'A': |
YusukeWakuta | 0:c7d5470d50e9 | 71 | servo_deg1 = rs485.getc(); |
YusukeWakuta | 0:c7d5470d50e9 | 72 | break; |
YusukeWakuta | 0:c7d5470d50e9 | 73 | case 'B': |
YusukeWakuta | 0:c7d5470d50e9 | 74 | servo_deg2 = rs485.getc(); |
YusukeWakuta | 0:c7d5470d50e9 | 75 | break; |
YusukeWakuta | 0:c7d5470d50e9 | 76 | case 'P': |
YusukeWakuta | 1:ec22c326aa88 | 77 | INA226_check(Voltage,Current); |
YusukeWakuta | 1:ec22c326aa88 | 78 | ADXL345(acc); |
YusukeWakuta | 0:c7d5470d50e9 | 79 | Receiver = 1; |
YusukeWakuta | 0:c7d5470d50e9 | 80 | wait_ms(3); |
YusukeWakuta | 1:ec22c326aa88 | 81 | rs485.putc('X'); |
YusukeWakuta | 0:c7d5470d50e9 | 82 | rs485.putc((signed char)acc[0]); |
YusukeWakuta | 0:c7d5470d50e9 | 83 | rs485.putc('Y'); |
YusukeWakuta | 0:c7d5470d50e9 | 84 | rs485.putc((signed char)acc[1]); |
YusukeWakuta | 0:c7d5470d50e9 | 85 | rs485.putc('Z'); |
YusukeWakuta | 0:c7d5470d50e9 | 86 | rs485.putc((signed char)acc[2]); |
YusukeWakuta | 0:c7d5470d50e9 | 87 | rs485.putc('V'); |
YusukeWakuta | 0:c7d5470d50e9 | 88 | pc.printf("V"); |
YusukeWakuta | 0:c7d5470d50e9 | 89 | rs485.putc((signed char)Voltage); |
YusukeWakuta | 0:c7d5470d50e9 | 90 | pc.printf("%5d",(signed char)Voltage); |
YusukeWakuta | 0:c7d5470d50e9 | 91 | rs485.putc('C'); |
YusukeWakuta | 0:c7d5470d50e9 | 92 | pc.printf("C"); |
YusukeWakuta | 0:c7d5470d50e9 | 93 | rs485.putc((signed char)Current); |
YusukeWakuta | 0:c7d5470d50e9 | 94 | pc.printf("%5d\n\r",(signed char)Current); |
YusukeWakuta | 0:c7d5470d50e9 | 95 | wait_ms(15); |
YusukeWakuta | 0:c7d5470d50e9 | 96 | Receiver = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 97 | //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]); |
YusukeWakuta | 0:c7d5470d50e9 | 98 | break; |
YusukeWakuta | 0:c7d5470d50e9 | 99 | default: |
YusukeWakuta | 0:c7d5470d50e9 | 100 | wait_us(5); |
YusukeWakuta | 0:c7d5470d50e9 | 101 | } |
YusukeWakuta | 0:c7d5470d50e9 | 102 | } |
YusukeWakuta | 0:c7d5470d50e9 | 103 | |
YusukeWakuta | 0:c7d5470d50e9 | 104 | void init() |
YusukeWakuta | 0:c7d5470d50e9 | 105 | { |
YusukeWakuta | 0:c7d5470d50e9 | 106 | Receiver = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 107 | pc.printf("Receiver\n\r"); |
YusukeWakuta | 0:c7d5470d50e9 | 108 | |
YusukeWakuta | 0:c7d5470d50e9 | 109 | rs485.baud(38400); |
YusukeWakuta | 0:c7d5470d50e9 | 110 | rs485.attach(rs485_rx, Serial::RxIrq); |
YusukeWakuta | 0:c7d5470d50e9 | 111 | |
YusukeWakuta | 0:c7d5470d50e9 | 112 | servo1.period_ms(20); |
YusukeWakuta | 0:c7d5470d50e9 | 113 | servo2.period_ms(20); |
YusukeWakuta | 0:c7d5470d50e9 | 114 | |
YusukeWakuta | 0:c7d5470d50e9 | 115 | accelerometer.setPowerControl(0x00); |
YusukeWakuta | 0:c7d5470d50e9 | 116 | accelerometer.setDataFormatControl(0x0B); |
YusukeWakuta | 0:c7d5470d50e9 | 117 | accelerometer.setDataRate(ADXL345_3200HZ); |
YusukeWakuta | 0:c7d5470d50e9 | 118 | accelerometer.setPowerControl(0x08); |
YusukeWakuta | 0:c7d5470d50e9 | 119 | } |
YusukeWakuta | 0:c7d5470d50e9 | 120 | |
YusukeWakuta | 0:c7d5470d50e9 | 121 | |
YusukeWakuta | 0:c7d5470d50e9 | 122 | int main() |
YusukeWakuta | 0:c7d5470d50e9 | 123 | { |
YusukeWakuta | 0:c7d5470d50e9 | 124 | init(); |
YusukeWakuta | 0:c7d5470d50e9 | 125 | while(1) { |
YusukeWakuta | 0:c7d5470d50e9 | 126 | servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077)); |
YusukeWakuta | 0:c7d5470d50e9 | 127 | servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077)); |
YusukeWakuta | 0:c7d5470d50e9 | 128 | wait_ms(10); |
YusukeWakuta | 0:c7d5470d50e9 | 129 | } |
YusukeWakuta | 0:c7d5470d50e9 | 130 | } |