albatross / Mbed 2 deprecated Control_R_ver1

1/23 操舵　レシーバ

Dependencies:   ADXL345_I2C INA226_ver1 mbed

Fork of RS485R_2 by albatross

main.cpp@2:2b98a651b0a1, 2015-12-05 (annotated)

Committer:
taurin
Date:
Sat Dec 05 11:21:06 2015 +0000
Revision:
2:2b98a651b0a1
Parent:
1:563b7d0b896f
Child:
3:2bdb60bbc665
????????????????????????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taurin 0:fafac0045ad3 1 //相互通信確認用
taurin 0:fafac0045ad3 2 #include "mbed.h"
taurin 1:563b7d0b896f 3 #include "ADXL345_I2C.h"
taurin 0:fafac0045ad3 4 #define BUFFER 30
taurin 0:fafac0045ad3 5
taurin 1:563b7d0b896f 6 Serial rs485(p13,p14);
taurin 0:fafac0045ad3 7 Serial pc(USBTX,USBRX);
taurin 0:fafac0045ad3 8 DigitalOut Receiver(p5);
taurin 1:563b7d0b896f 9 ADXL345_I2C accelerometer(p9, p10);
taurin 0:fafac0045ad3 10 PwmOut servo1(p21);
taurin 0:fafac0045ad3 11 PwmOut servo2(p22);
taurin 0:fafac0045ad3 12
taurin 0:fafac0045ad3 13 int counter = 0;
taurin 0:fafac0045ad3 14 int servo_deg1 = 0;
taurin 0:fafac0045ad3 15 int servo_deg2 = 0;
taurin 0:fafac0045ad3 16
taurin 1:563b7d0b896f 17 int acc[3] = {0,0,0};
taurin 1:563b7d0b896f 18
taurin 0:fafac0045ad3 19 void rs485_rx()
taurin 0:fafac0045ad3 20 {
taurin 2:2b98a651b0a1 21 signed char rec_data = rs485.getc();
taurin 1:563b7d0b896f 22 switch(rec_data) {
taurin 1:563b7d0b896f 23 case 'A':
taurin 1:563b7d0b896f 24 servo_deg1 = rs485.getc();
taurin 1:563b7d0b896f 25 pc.printf("counter1:%d\n\r",servo_deg1);
taurin 1:563b7d0b896f 26 break;
taurin 1:563b7d0b896f 27 case 'B':
taurin 1:563b7d0b896f 28 servo_deg2 = rs485.getc();
taurin 2:2b98a651b0a1 29 //pc.printf("counter2:%d\n\r",servo_deg2);
taurin 1:563b7d0b896f 30 break;
taurin 1:563b7d0b896f 31 case 'C':
taurin 1:563b7d0b896f 32 Receiver = 1;
taurin 1:563b7d0b896f 33 wait_ms(1);
taurin 1:563b7d0b896f 34 rs485.putc('X');
taurin 2:2b98a651b0a1 35 rs485.putc((signed char)acc[0]);
taurin 1:563b7d0b896f 36 rs485.putc('Y');
taurin 2:2b98a651b0a1 37 rs485.putc((signed char)acc[1]);
taurin 1:563b7d0b896f 38 rs485.putc('Z');
taurin 2:2b98a651b0a1 39 rs485.putc((signed char)acc[2]);
taurin 2:2b98a651b0a1 40 // rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]);
taurin 1:563b7d0b896f 41 wait_ms(2);
taurin 1:563b7d0b896f 42 Receiver = 0;
taurin 2:2b98a651b0a1 43 //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
taurin 1:563b7d0b896f 44 break;
taurin 1:563b7d0b896f 45 default:
taurin 1:563b7d0b896f 46 wait_us(5);
taurin 1:563b7d0b896f 47 pc.printf("%d\n\r",rec_data);
taurin 0:fafac0045ad3 48 }
taurin 0:fafac0045ad3 49 }
taurin 0:fafac0045ad3 50
taurin 1:563b7d0b896f 51 void init()
taurin 1:563b7d0b896f 52 {
taurin 1:563b7d0b896f 53 Receiver = 0;
taurin 1:563b7d0b896f 54 pc.printf("Receiver\n\r");
taurin 1:563b7d0b896f 55
taurin 1:563b7d0b896f 56 rs485.baud(38400);
taurin 1:563b7d0b896f 57 rs485.attach(rs485_rx, Serial::RxIrq);
taurin 1:563b7d0b896f 58
taurin 1:563b7d0b896f 59 servo1.period_ms(20);
taurin 1:563b7d0b896f 60 servo2.period_ms(20);
taurin 1:563b7d0b896f 61
taurin 1:563b7d0b896f 62 accelerometer.setPowerControl(0x00);
taurin 1:563b7d0b896f 63 accelerometer.setDataFormatControl(0x0B);
taurin 1:563b7d0b896f 64 accelerometer.setDataRate(ADXL345_3200HZ);
taurin 1:563b7d0b896f 65 accelerometer.setPowerControl(0x08);
taurin 1:563b7d0b896f 66 }
taurin 1:563b7d0b896f 67
taurin 0:fafac0045ad3 68 int main()
taurin 0:fafac0045ad3 69 {
taurin 1:563b7d0b896f 70 init();
taurin 1:563b7d0b896f 71 while(1) {
taurin 1:563b7d0b896f 72 accelerometer.getOutput(acc);
taurin 0:fafac0045ad3 73 servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
taurin 0:fafac0045ad3 74 servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
taurin 2:2b98a651b0a1 75 wait_ms(10);
taurin 0:fafac0045ad3 76 }
taurin 0:fafac0045ad3 77 }