albatross / Mbed 2 deprecated SOUDA_YOKUTAN_R

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

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?

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