albatross / Mbed 2 deprecated SOUDA_YOKUTAN_L

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

Dependencies:   ADXL345_I2C INA226_120 mbed

main.cpp@1:497ad7f0e39b, 2016-01-20 (annotated)

Committer:
YusukeWakuta
Date:
Wed Jan 20 13:42:30 2016 +0000
Revision:
1:497ad7f0e39b
Parent:
0:fe607370807b 
1?20????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YusukeWakuta 0:fe607370807b 1 #include "mbed.h"
YusukeWakuta 0:fe607370807b 2 #include "ADXL345_I2C.h"
YusukeWakuta 0:fe607370807b 3 #include "INA226.hpp"
YusukeWakuta 0:fe607370807b 4 #define BUFFER 30
YusukeWakuta 0:fe607370807b 5
YusukeWakuta 1:497ad7f0e39b 6
YusukeWakuta 0:fe607370807b 7 Serial rs485(p13,p14);
YusukeWakuta 0:fe607370807b 8 DigitalOut Receiver(p5);
YusukeWakuta 0:fe607370807b 9 ADXL345_I2C accelerometer(p9, p10);
YusukeWakuta 0:fe607370807b 10 PwmOut servo1(p21);
YusukeWakuta 0:fe607370807b 11 PwmOut servo2(p22);
YusukeWakuta 0:fe607370807b 12 I2C i2c_Ina(p28,p27);
YusukeWakuta 0:fe607370807b 13 INA226 Ina226(i2c_Ina);
YusukeWakuta 1:497ad7f0e39b 14
YusukeWakuta 0:fe607370807b 15 Serial pc(USBTX, USBRX);
YusukeWakuta 1:497ad7f0e39b 16
YusukeWakuta 0:fe607370807b 17 void ADXL345(int *x)
YusukeWakuta 0:fe607370807b 18 {
YusukeWakuta 1:497ad7f0e39b 19
YusukeWakuta 1:497ad7f0e39b 20 // pc.printf("Starting ADXL345 test...\n");
YusukeWakuta 0:fe607370807b 21 //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
YusukeWakuta 1:497ad7f0e39b 22
YusukeWakuta 0:fe607370807b 23 //Go into standby mode to configure the device.
YusukeWakuta 0:fe607370807b 24 accelerometer.setPowerControl(0x00);
YusukeWakuta 1:497ad7f0e39b 25
YusukeWakuta 0:fe607370807b 26 //Full resolution, +/-16g, 4mg/LSB.
YusukeWakuta 0:fe607370807b 27 accelerometer.setDataFormatControl(0x0B);
YusukeWakuta 1:497ad7f0e39b 28
YusukeWakuta 0:fe607370807b 29 //3.2kHz data rate.
YusukeWakuta 0:fe607370807b 30 accelerometer.setDataRate(ADXL345_3200HZ);
YusukeWakuta 1:497ad7f0e39b 31
YusukeWakuta 0:fe607370807b 32 //Measurement mode.
YusukeWakuta 0:fe607370807b 33 accelerometer.setPowerControl(0x08);
YusukeWakuta 0:fe607370807b 34
YusukeWakuta 1:497ad7f0e39b 35 accelerometer.getOutput(x);
YusukeWakuta 1:497ad7f0e39b 36
YusukeWakuta 1:497ad7f0e39b 37 //13-bit, sign extended values.
YusukeWakuta 1:497ad7f0e39b 38 //pc.printf("%6i, %6i, %6i\n\r", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
YusukeWakuta 1:497ad7f0e39b 39
YusukeWakuta 0:fe607370807b 40 }
YusukeWakuta 0:fe607370807b 41
YusukeWakuta 1:497ad7f0e39b 42 int INA226_check(double *Voltage,double *Current)
YusukeWakuta 1:497ad7f0e39b 43 {
YusukeWakuta 1:497ad7f0e39b 44 while(1) {
YusukeWakuta 1:497ad7f0e39b 45 if(Ina226.isExist() == 0) {
YusukeWakuta 1:497ad7f0e39b 46 Ina226.getVoltage(Voltage);
YusukeWakuta 1:497ad7f0e39b 47 Ina226.getCurrent(Current);
YusukeWakuta 0:fe607370807b 48 }
YusukeWakuta 1:497ad7f0e39b 49 //pc.printf("V : %lf, C : %lf \n\r",Voltage,Current);
YusukeWakuta 1:497ad7f0e39b 50
YusukeWakuta 0:fe607370807b 51 }
YusukeWakuta 0:fe607370807b 52 return 0;
YusukeWakuta 1:497ad7f0e39b 53 }
YusukeWakuta 0:fe607370807b 54
YusukeWakuta 0:fe607370807b 55
YusukeWakuta 0:fe607370807b 56
YusukeWakuta 0:fe607370807b 57 int counter = 0;
YusukeWakuta 0:fe607370807b 58 int servo_deg1 = 0;
YusukeWakuta 0:fe607370807b 59 int servo_deg2 = 0;
YusukeWakuta 0:fe607370807b 60
YusukeWakuta 0:fe607370807b 61 int acc[3] = {0,0,0};
YusukeWakuta 0:fe607370807b 62
YusukeWakuta 0:fe607370807b 63 void rs485_rx()
YusukeWakuta 0:fe607370807b 64 {
YusukeWakuta 0:fe607370807b 65 int acc[3];
YusukeWakuta 1:497ad7f0e39b 66 double *Voltage = 0;
YusukeWakuta 1:497ad7f0e39b 67 double *Current = 0;
YusukeWakuta 0:fe607370807b 68 signed char rec_data = rs485.getc();
YusukeWakuta 0:fe607370807b 69 switch(rec_data) {
YusukeWakuta 0:fe607370807b 70 case 'C':
YusukeWakuta 0:fe607370807b 71 servo_deg1 = rs485.getc();
YusukeWakuta 1:497ad7f0e39b 72 // pc.printf("counter1:%d\n\r",servo_deg1);
YusukeWakuta 0:fe607370807b 73 break;
YusukeWakuta 0:fe607370807b 74 case 'D':
YusukeWakuta 0:fe607370807b 75 servo_deg2 = rs485.getc();
YusukeWakuta 0:fe607370807b 76 //pc.printf("counter2:%d\n\r",servo_deg2);
YusukeWakuta 0:fe607370807b 77 break;
YusukeWakuta 0:fe607370807b 78 case 'P':
YusukeWakuta 1:497ad7f0e39b 79 INA226_check(Voltage,Current);
YusukeWakuta 1:497ad7f0e39b 80 ADXL345(acc);
YusukeWakuta 0:fe607370807b 81 Receiver = 1;
YusukeWakuta 0:fe607370807b 82 wait_ms(3);
YusukeWakuta 1:497ad7f0e39b 83 rs485.putc('X');
YusukeWakuta 1:497ad7f0e39b 84 pc.printf("X");
YusukeWakuta 0:fe607370807b 85 rs485.putc((signed char)acc[0]);
YusukeWakuta 1:497ad7f0e39b 86 pc.printf("%5d",(signed char)acc[0]);
YusukeWakuta 0:fe607370807b 87 rs485.putc('Y');
YusukeWakuta 1:497ad7f0e39b 88 pc.printf("Y");
YusukeWakuta 0:fe607370807b 89 rs485.putc((signed char)acc[1]);
YusukeWakuta 1:497ad7f0e39b 90 pc.printf("%5d",(signed char)acc[1]);
YusukeWakuta 0:fe607370807b 91 rs485.putc('Z');
YusukeWakuta 1:497ad7f0e39b 92 pc.printf("Z");
YusukeWakuta 1:497ad7f0e39b 93 pc.printf("%5d",(signed char)acc[2]);
YusukeWakuta 0:fe607370807b 94 rs485.putc('V');
YusukeWakuta 0:fe607370807b 95 pc.printf("V");
YusukeWakuta 0:fe607370807b 96 rs485.putc((signed char)Voltage);
YusukeWakuta 0:fe607370807b 97 pc.printf("%5d",(signed char)Voltage);
YusukeWakuta 0:fe607370807b 98 rs485.putc('C');
YusukeWakuta 0:fe607370807b 99 pc.printf("C");
YusukeWakuta 0:fe607370807b 100 rs485.putc((signed char)Current);
YusukeWakuta 0:fe607370807b 101 pc.printf("%5d\n\r",(signed char)Current);
YusukeWakuta 1:497ad7f0e39b 102
YusukeWakuta 0:fe607370807b 103 //pc.printf("\n\r");
YusukeWakuta 0:fe607370807b 104 // rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]);
YusukeWakuta 0:fe607370807b 105 wait_ms(15);
YusukeWakuta 0:fe607370807b 106 Receiver = 0;
YusukeWakuta 0:fe607370807b 107 //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
YusukeWakuta 0:fe607370807b 108 break;
YusukeWakuta 0:fe607370807b 109 default:
YusukeWakuta 0:fe607370807b 110 wait_us(5);
YusukeWakuta 1:497ad7f0e39b 111 // pc.printf("%d\n\r",rec_data);
YusukeWakuta 0:fe607370807b 112 }
YusukeWakuta 0:fe607370807b 113 }
YusukeWakuta 0:fe607370807b 114
YusukeWakuta 0:fe607370807b 115 void init()
YusukeWakuta 0:fe607370807b 116 {
YusukeWakuta 0:fe607370807b 117 Receiver = 0;
YusukeWakuta 0:fe607370807b 118 pc.printf("Receiver\n\r");
YusukeWakuta 0:fe607370807b 119
YusukeWakuta 0:fe607370807b 120 rs485.baud(38400);
YusukeWakuta 0:fe607370807b 121 rs485.attach(rs485_rx, Serial::RxIrq);
YusukeWakuta 0:fe607370807b 122
YusukeWakuta 0:fe607370807b 123 servo1.period_ms(20);
YusukeWakuta 0:fe607370807b 124 servo2.period_ms(20);
YusukeWakuta 0:fe607370807b 125
YusukeWakuta 0:fe607370807b 126 accelerometer.setPowerControl(0x00);
YusukeWakuta 0:fe607370807b 127 accelerometer.setDataFormatControl(0x0B);
YusukeWakuta 0:fe607370807b 128 accelerometer.setDataRate(ADXL345_3200HZ);
YusukeWakuta 0:fe607370807b 129 accelerometer.setPowerControl(0x08);
YusukeWakuta 0:fe607370807b 130 }
YusukeWakuta 0:fe607370807b 131
YusukeWakuta 0:fe607370807b 132
YusukeWakuta 0:fe607370807b 133 int main()
YusukeWakuta 0:fe607370807b 134 {
YusukeWakuta 0:fe607370807b 135 init();
YusukeWakuta 0:fe607370807b 136 while(1) {
YusukeWakuta 0:fe607370807b 137 servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
YusukeWakuta 0:fe607370807b 138 servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
YusukeWakuta 0:fe607370807b 139 wait_ms(10);
YusukeWakuta 0:fe607370807b 140 }
YusukeWakuta 0:fe607370807b 141 }