albatross / Mbed 2 deprecated SOUDA_YOKUTAN_R

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

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?

UserRevisionLine numberNew 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 }