操舵ソースコードの第一稿です。 右翼端です。
Dependencies: ADXL345_I2C INA226 mbed
main.cpp@0:c7d5470d50e9, 2016-01-15 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Fri Jan 15 15:30:00 2016 +0000
- Revision:
- 0:c7d5470d50e9
- Child:
- 1:ec22c326aa88
???????????; ??????
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 | 0:c7d5470d50e9 | 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 | 0:c7d5470d50e9 | 14 | |
YusukeWakuta | 0:c7d5470d50e9 | 15 | //ADXL345 accelerometer(p5, p6, p7, p8); |
YusukeWakuta | 0:c7d5470d50e9 | 16 | ADXL345_I2C accelerometer(p9, p10); |
YusukeWakuta | 0:c7d5470d50e9 | 17 | Serial pc(USBTX, USBRX); |
YusukeWakuta | 0:c7d5470d50e9 | 18 | |
YusukeWakuta | 0:c7d5470d50e9 | 19 | void ADXL345(int *x) |
YusukeWakuta | 0:c7d5470d50e9 | 20 | { |
YusukeWakuta | 0:c7d5470d50e9 | 21 | x[3] = {0, 0, 0}; |
YusukeWakuta | 0:c7d5470d50e9 | 22 | |
YusukeWakuta | 0:c7d5470d50e9 | 23 | // pc.printf("Starting ADXL345 test...\n"); |
YusukeWakuta | 0:c7d5470d50e9 | 24 | //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId()); |
YusukeWakuta | 0:c7d5470d50e9 | 25 | |
YusukeWakuta | 0:c7d5470d50e9 | 26 | //Go into standby mode to configure the device. |
YusukeWakuta | 0:c7d5470d50e9 | 27 | accelerometer.setPowerControl(0x00); |
YusukeWakuta | 0:c7d5470d50e9 | 28 | |
YusukeWakuta | 0:c7d5470d50e9 | 29 | //Full resolution, +/-16g, 4mg/LSB. |
YusukeWakuta | 0:c7d5470d50e9 | 30 | accelerometer.setDataFormatControl(0x0B); |
YusukeWakuta | 0:c7d5470d50e9 | 31 | |
YusukeWakuta | 0:c7d5470d50e9 | 32 | //3.2kHz data rate. |
YusukeWakuta | 0:c7d5470d50e9 | 33 | accelerometer.setDataRate(ADXL345_3200HZ); |
YusukeWakuta | 0:c7d5470d50e9 | 34 | |
YusukeWakuta | 0:c7d5470d50e9 | 35 | //Measurement mode. |
YusukeWakuta | 0:c7d5470d50e9 | 36 | accelerometer.setPowerControl(0x08); |
YusukeWakuta | 0:c7d5470d50e9 | 37 | |
YusukeWakuta | 0:c7d5470d50e9 | 38 | accelerometer.getOutput(x); |
YusukeWakuta | 0:c7d5470d50e9 | 39 | |
YusukeWakuta | 0:c7d5470d50e9 | 40 | //13-bit, sign extended values. |
YusukeWakuta | 0:c7d5470d50e9 | 41 | //pc.printf("%6i, %6i, %6i\n\r", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]); |
YusukeWakuta | 0:c7d5470d50e9 | 42 | |
YusukeWakuta | 0:c7d5470d50e9 | 43 | } |
YusukeWakuta | 0:c7d5470d50e9 | 44 | |
YusukeWakuta | 0:c7d5470d50e9 | 45 | int INA226(double *Voltage,double *Current){ |
YusukeWakuta | 0:c7d5470d50e9 | 46 | while(1){ |
YusukeWakuta | 0:c7d5470d50e9 | 47 | if(Ina226.isExist() == 0){ |
YusukeWakuta | 0:c7d5470d50e9 | 48 | Ina226.getVoltage(Voltage); |
YusukeWakuta | 0:c7d5470d50e9 | 49 | Ina226.getCurrent(Current); |
YusukeWakuta | 0:c7d5470d50e9 | 50 | } |
YusukeWakuta | 0:c7d5470d50e9 | 51 | //pc.printf("V : %lf, C : %lf \n\r",Voltage,Current); |
YusukeWakuta | 0:c7d5470d50e9 | 52 | |
YusukeWakuta | 0:c7d5470d50e9 | 53 | } |
YusukeWakuta | 0:c7d5470d50e9 | 54 | return 0; |
YusukeWakuta | 0:c7d5470d50e9 | 55 | } |
YusukeWakuta | 0:c7d5470d50e9 | 56 | |
YusukeWakuta | 0:c7d5470d50e9 | 57 | |
YusukeWakuta | 0:c7d5470d50e9 | 58 | |
YusukeWakuta | 0:c7d5470d50e9 | 59 | int counter = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 60 | int servo_deg1 = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 61 | int servo_deg2 = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 62 | |
YusukeWakuta | 0:c7d5470d50e9 | 63 | int acc[3] = {0,0,0}; |
YusukeWakuta | 0:c7d5470d50e9 | 64 | |
YusukeWakuta | 0:c7d5470d50e9 | 65 | void rs485_rx() |
YusukeWakuta | 0:c7d5470d50e9 | 66 | { |
YusukeWakuta | 0:c7d5470d50e9 | 67 | int acc[3]; |
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 | // pc.printf("counter1:%d\n\r",servo_deg1); |
YusukeWakuta | 0:c7d5470d50e9 | 73 | break; |
YusukeWakuta | 0:c7d5470d50e9 | 74 | case 'B': |
YusukeWakuta | 0:c7d5470d50e9 | 75 | servo_deg2 = rs485.getc(); |
YusukeWakuta | 0:c7d5470d50e9 | 76 | //pc.printf("counter2:%d\n\r",servo_deg2); |
YusukeWakuta | 0:c7d5470d50e9 | 77 | break; |
YusukeWakuta | 0:c7d5470d50e9 | 78 | case 'P': |
YusukeWakuta | 0:c7d5470d50e9 | 79 | INA226_check(Voltage,Current); |
YusukeWakuta | 0:c7d5470d50e9 | 80 | ADXL345(acc); |
YusukeWakuta | 0:c7d5470d50e9 | 81 | //counter++; |
YusukeWakuta | 0:c7d5470d50e9 | 82 | // if(counter > 1000){ |
YusukeWakuta | 0:c7d5470d50e9 | 83 | // counter = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 84 | // } |
YusukeWakuta | 0:c7d5470d50e9 | 85 | Receiver = 1; |
YusukeWakuta | 0:c7d5470d50e9 | 86 | wait_ms(3); |
YusukeWakuta | 0:c7d5470d50e9 | 87 | // if(counter % 2 == 0){ |
YusukeWakuta | 0:c7d5470d50e9 | 88 | rs485.putc('X'); |
YusukeWakuta | 0:c7d5470d50e9 | 89 | // pc.printf("X"); |
YusukeWakuta | 0:c7d5470d50e9 | 90 | rs485.putc((signed char)acc[0]); |
YusukeWakuta | 0:c7d5470d50e9 | 91 | //pc.printf("%5d",(signed char)acc[0]); |
YusukeWakuta | 0:c7d5470d50e9 | 92 | rs485.putc('Y'); |
YusukeWakuta | 0:c7d5470d50e9 | 93 | //pc.printf("Y"); |
YusukeWakuta | 0:c7d5470d50e9 | 94 | rs485.putc((signed char)acc[1]); |
YusukeWakuta | 0:c7d5470d50e9 | 95 | // pc.printf("%5d",(signed char)acc[1]); |
YusukeWakuta | 0:c7d5470d50e9 | 96 | rs485.putc('Z'); |
YusukeWakuta | 0:c7d5470d50e9 | 97 | //pc.printf("Z"); |
YusukeWakuta | 0:c7d5470d50e9 | 98 | rs485.putc((signed char)acc[2]); |
YusukeWakuta | 0:c7d5470d50e9 | 99 | //} |
YusukeWakuta | 0:c7d5470d50e9 | 100 | // else{ |
YusukeWakuta | 0:c7d5470d50e9 | 101 | // pc.printf("%5d",(signed char)acc[2]); |
YusukeWakuta | 0:c7d5470d50e9 | 102 | rs485.putc('V'); |
YusukeWakuta | 0:c7d5470d50e9 | 103 | pc.printf("V"); |
YusukeWakuta | 0:c7d5470d50e9 | 104 | rs485.putc((signed char)Voltage); |
YusukeWakuta | 0:c7d5470d50e9 | 105 | pc.printf("%5d",(signed char)Voltage); |
YusukeWakuta | 0:c7d5470d50e9 | 106 | rs485.putc('C'); |
YusukeWakuta | 0:c7d5470d50e9 | 107 | pc.printf("C"); |
YusukeWakuta | 0:c7d5470d50e9 | 108 | rs485.putc((signed char)Current); |
YusukeWakuta | 0:c7d5470d50e9 | 109 | pc.printf("%5d\n\r",(signed char)Current); |
YusukeWakuta | 0:c7d5470d50e9 | 110 | //} |
YusukeWakuta | 0:c7d5470d50e9 | 111 | //pc.printf("\n\r"); |
YusukeWakuta | 0:c7d5470d50e9 | 112 | // rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]); |
YusukeWakuta | 0:c7d5470d50e9 | 113 | wait_ms(15); |
YusukeWakuta | 0:c7d5470d50e9 | 114 | Receiver = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 115 | //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 | 116 | break; |
YusukeWakuta | 0:c7d5470d50e9 | 117 | default: |
YusukeWakuta | 0:c7d5470d50e9 | 118 | wait_us(5); |
YusukeWakuta | 0:c7d5470d50e9 | 119 | // pc.printf("%d\n\r",rec_data); |
YusukeWakuta | 0:c7d5470d50e9 | 120 | } |
YusukeWakuta | 0:c7d5470d50e9 | 121 | } |
YusukeWakuta | 0:c7d5470d50e9 | 122 | |
YusukeWakuta | 0:c7d5470d50e9 | 123 | void init() |
YusukeWakuta | 0:c7d5470d50e9 | 124 | { |
YusukeWakuta | 0:c7d5470d50e9 | 125 | Receiver = 0; |
YusukeWakuta | 0:c7d5470d50e9 | 126 | pc.printf("Receiver\n\r"); |
YusukeWakuta | 0:c7d5470d50e9 | 127 | |
YusukeWakuta | 0:c7d5470d50e9 | 128 | rs485.baud(38400); |
YusukeWakuta | 0:c7d5470d50e9 | 129 | rs485.attach(rs485_rx, Serial::RxIrq); |
YusukeWakuta | 0:c7d5470d50e9 | 130 | |
YusukeWakuta | 0:c7d5470d50e9 | 131 | servo1.period_ms(20); |
YusukeWakuta | 0:c7d5470d50e9 | 132 | servo2.period_ms(20); |
YusukeWakuta | 0:c7d5470d50e9 | 133 | |
YusukeWakuta | 0:c7d5470d50e9 | 134 | accelerometer.setPowerControl(0x00); |
YusukeWakuta | 0:c7d5470d50e9 | 135 | accelerometer.setDataFormatControl(0x0B); |
YusukeWakuta | 0:c7d5470d50e9 | 136 | accelerometer.setDataRate(ADXL345_3200HZ); |
YusukeWakuta | 0:c7d5470d50e9 | 137 | accelerometer.setPowerControl(0x08); |
YusukeWakuta | 0:c7d5470d50e9 | 138 | } |
YusukeWakuta | 0:c7d5470d50e9 | 139 | |
YusukeWakuta | 0:c7d5470d50e9 | 140 | |
YusukeWakuta | 0:c7d5470d50e9 | 141 | int main() |
YusukeWakuta | 0:c7d5470d50e9 | 142 | { |
YusukeWakuta | 0:c7d5470d50e9 | 143 | init(); |
YusukeWakuta | 0:c7d5470d50e9 | 144 | while(1) { |
YusukeWakuta | 0:c7d5470d50e9 | 145 | servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077)); |
YusukeWakuta | 0:c7d5470d50e9 | 146 | servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077)); |
YusukeWakuta | 0:c7d5470d50e9 | 147 | wait_ms(10); |
YusukeWakuta | 0:c7d5470d50e9 | 148 | } |
YusukeWakuta | 0:c7d5470d50e9 | 149 | } |