albatross / Mbed 2 deprecated Control_R_ver1

1/23 操舵　レシーバ

Dependencies:   ADXL345_I2C INA226_ver1 mbed

Fork of RS485R_2 by albatross

main.cpp@3:2bdb60bbc665, 2016-01-23 (annotated)

Committer:
taurin
Date:
Sat Jan 23 10:43:15 2016 +0000
Revision:
3:2bdb60bbc665
Parent:
2:2b98a651b0a1 
1/23 ???????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taurin 3:2bdb60bbc665 1 //翼端操舵プログラム
taurin 0:fafac0045ad3 2 #include "mbed.h"
taurin 1:563b7d0b896f 3 #include "ADXL345_I2C.h"
taurin 3:2bdb60bbc665 4 #include "INA226.hpp"
taurin 0:fafac0045ad3 5 #define BUFFER 30
taurin 0:fafac0045ad3 6
taurin 1:563b7d0b896f 7 Serial rs485(p13,p14);
taurin 0:fafac0045ad3 8 Serial pc(USBTX,USBRX);
taurin 0:fafac0045ad3 9 DigitalOut Receiver(p5);
taurin 1:563b7d0b896f 10 ADXL345_I2C accelerometer(p9, p10);
taurin 3:2bdb60bbc665 11 I2C ina226_i2c(p28,p27);
taurin 3:2bdb60bbc665 12 INA226 VCmonitor(ina226_i2c);
taurin 0:fafac0045ad3 13 PwmOut servo1(p21);
taurin 0:fafac0045ad3 14 PwmOut servo2(p22);
taurin 0:fafac0045ad3 15
taurin 0:fafac0045ad3 16 int counter = 0;
taurin 3:2bdb60bbc665 17 int eruron_deg = 0;
taurin 3:2bdb60bbc665 18 int drug_deg = 0;
taurin 3:2bdb60bbc665 19 unsigned short ina_val;
taurin 3:2bdb60bbc665 20 double V,C;
taurin 0:fafac0045ad3 21
taurin 1:563b7d0b896f 22 int acc[3] = {0,0,0};
taurin 1:563b7d0b896f 23
taurin 0:fafac0045ad3 24 void rs485_rx()
taurin 0:fafac0045ad3 25 {
taurin 2:2b98a651b0a1 26 signed char rec_data = rs485.getc();
taurin 1:563b7d0b896f 27 switch(rec_data) {
taurin 1:563b7d0b896f 28 case 'A':
taurin 3:2bdb60bbc665 29 eruron_deg = rs485.getc();
taurin 1:563b7d0b896f 30 break;
taurin 1:563b7d0b896f 31 case 'B':
taurin 3:2bdb60bbc665 32 drug_deg = rs485.getc();
taurin 1:563b7d0b896f 33 break;
taurin 1:563b7d0b896f 34 case 'C':
taurin 1:563b7d0b896f 35 Receiver = 1;
taurin 1:563b7d0b896f 36 wait_ms(1);
taurin 1:563b7d0b896f 37 rs485.putc('X');
taurin 2:2b98a651b0a1 38 rs485.putc((signed char)acc[0]);
taurin 1:563b7d0b896f 39 rs485.putc('Y');
taurin 2:2b98a651b0a1 40 rs485.putc((signed char)acc[1]);
taurin 1:563b7d0b896f 41 rs485.putc('Z');
taurin 2:2b98a651b0a1 42 rs485.putc((signed char)acc[2]);
taurin 3:2bdb60bbc665 43 rs485.putc('V');
taurin 3:2bdb60bbc665 44 rs485.putc((signed char)V);
taurin 3:2bdb60bbc665 45 rs485.putc('I');
taurin 3:2bdb60bbc665 46 rs485.putc((signed char)C);
taurin 1:563b7d0b896f 47 wait_ms(2);
taurin 1:563b7d0b896f 48 Receiver = 0;
taurin 1:563b7d0b896f 49 break;
taurin 1:563b7d0b896f 50 default:
taurin 1:563b7d0b896f 51 wait_us(5);
taurin 0:fafac0045ad3 52 }
taurin 0:fafac0045ad3 53 }
taurin 0:fafac0045ad3 54
taurin 1:563b7d0b896f 55 void init()
taurin 1:563b7d0b896f 56 {
taurin 1:563b7d0b896f 57 Receiver = 0;
taurin 1:563b7d0b896f 58 pc.printf("Receiver\n\r");
taurin 1:563b7d0b896f 59
taurin 1:563b7d0b896f 60 rs485.baud(38400);
taurin 1:563b7d0b896f 61 rs485.attach(rs485_rx, Serial::RxIrq);
taurin 1:563b7d0b896f 62
taurin 1:563b7d0b896f 63 servo1.period_ms(20);
taurin 1:563b7d0b896f 64 servo2.period_ms(20);
taurin 1:563b7d0b896f 65
taurin 1:563b7d0b896f 66 accelerometer.setPowerControl(0x00);
taurin 1:563b7d0b896f 67 accelerometer.setDataFormatControl(0x0B);
taurin 1:563b7d0b896f 68 accelerometer.setDataRate(ADXL345_3200HZ);
taurin 1:563b7d0b896f 69 accelerometer.setPowerControl(0x08);
taurin 3:2bdb60bbc665 70
taurin 3:2bdb60bbc665 71 if(!VCmonitor.isExist()){
taurin 3:2bdb60bbc665 72 pc.printf("VCmonitor NOT FOUND\n");
taurin 3:2bdb60bbc665 73 }
taurin 3:2bdb60bbc665 74 ina_val = 0;
taurin 3:2bdb60bbc665 75 if(VCmonitor.rawRead(0x00,&ina_val) != 0){
taurin 3:2bdb60bbc665 76 pc.printf("VCmonitor READ ERROR\n");
taurin 3:2bdb60bbc665 77 while(1){}
taurin 3:2bdb60bbc665 78 }
taurin 3:2bdb60bbc665 79 VCmonitor.setCurrentCalibration();
taurin 3:2bdb60bbc665 80 }
taurin 3:2bdb60bbc665 81
taurin 3:2bdb60bbc665 82 void updateDatas(){
taurin 3:2bdb60bbc665 83 accelerometer.getOutput(acc);
taurin 3:2bdb60bbc665 84 int tmp = VCmonitor.getVoltage(&V);
taurin 3:2bdb60bbc665 85 tmp = VCmonitor.getCurrent(&C);
taurin 3:2bdb60bbc665 86 }
taurin 3:2bdb60bbc665 87
taurin 3:2bdb60bbc665 88 double calcPulse(int deg){
taurin 3:2bdb60bbc665 89 return (0.00093+(deg/180.0)*(0.00235-0.00077));
taurin 3:2bdb60bbc665 90 }
taurin 3:2bdb60bbc665 91
taurin 3:2bdb60bbc665 92 void WriteServo(){
taurin 3:2bdb60bbc665 93 servo1.pulsewidth(calcPulse(eruron_deg));
taurin 3:2bdb60bbc665 94 servo2.pulsewidth(calcPulse(drug_deg));
taurin 3:2bdb60bbc665 95 }
taurin 3:2bdb60bbc665 96
taurin 3:2bdb60bbc665 97 void toStringDatas(){
taurin 3:2bdb60bbc665 98 pc.printf("acc[0]:%i acc[1]:%i acc[2]:%i\r\n",acc[0],acc[1],acc[2]);
taurin 3:2bdb60bbc665 99 pc.printf("V:%i C:%i\r\n",V,C);
taurin 3:2bdb60bbc665 100 pc.printf("\r\n");
taurin 1:563b7d0b896f 101 }
taurin 1:563b7d0b896f 102
taurin 0:fafac0045ad3 103 int main()
taurin 0:fafac0045ad3 104 {
taurin 1:563b7d0b896f 105 init();
taurin 1:563b7d0b896f 106 while(1) {
taurin 3:2bdb60bbc665 107 updateDatas();
taurin 3:2bdb60bbc665 108 WriteServo();
taurin 3:2bdb60bbc665 109 toStringDatas();
taurin 2:2b98a651b0a1 110 wait_ms(10);
taurin 0:fafac0045ad3 111 }
taurin 0:fafac0045ad3 112 }