albatross / Mbed 2 deprecated Souda_Yokutan_ver3

3/18 翼端操舵

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of ControlYokutan02 by albatross

main.cpp@6:a839ccbf7f9a, 2016-02-17 (annotated)

Committer:
YusukeWakuta
Date:
Wed Feb 17 02:43:40 2016 +0000
Revision:
6:a839ccbf7f9a
Parent:
3:4417217b4f66 
???????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taurin 0:e052602db102 1 //翼端can program
taurin 0:e052602db102 2 #include "mbed.h"
taurin 2:7fcb4f970a02 3 #define INIT_SERVO_PERIOD_MS 20
YusukeWakuta 3:4417217b4f66 4 #define WAIT_LOOP_TIME 0.2
taurin 2:7fcb4f970a02 5
taurin 0:e052602db102 6 Serial pc(USBTX,USBRX);
YusukeWakuta 3:4417217b4f66 7 PwmOut servo1(p22);
YusukeWakuta 6:a839ccbf7f9a 8 PwmOut servo2(p23);// eruron
taurin 1:9cc932a16d17 9 DigitalOut led1(LED1);
taurin 2:7fcb4f970a02 10 AnalogIn a(p20);
taurin 2:7fcb4f970a02 11 AnalogIn b(p19);
taurin 0:e052602db102 12
YusukeWakuta 6:a839ccbf7f9a 13 int neutral_a = 0;
YusukeWakuta 6:a839ccbf7f9a 14 int neutral_b = 0;
taurin 0:e052602db102 15
YusukeWakuta 6:a839ccbf7f9a 16 bool servoInit()
YusukeWakuta 6:a839ccbf7f9a 17 {
taurin 0:e052602db102 18 servo1.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 19 servo2.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 20 return true;
taurin 0:e052602db102 21 }
taurin 0:e052602db102 22
taurin 0:e052602db102 23
YusukeWakuta 6:a839ccbf7f9a 24 double calcPulse(int deg)
YusukeWakuta 6:a839ccbf7f9a 25 {
taurin 1:9cc932a16d17 26 return (0.00093+(deg/180.0)*(0.00235-0.00077));
taurin 1:9cc932a16d17 27 }
taurin 1:9cc932a16d17 28
YusukeWakuta 6:a839ccbf7f9a 29 void WriteServo(int a,int b)
YusukeWakuta 6:a839ccbf7f9a 30 {
YusukeWakuta 6:a839ccbf7f9a 31 servoInit();
YusukeWakuta 6:a839ccbf7f9a 32 servo1.pulsewidth(calcPulse(a * 90));
YusukeWakuta 6:a839ccbf7f9a 33 servo2.pulsewidth(calcPulse(b * 90));
YusukeWakuta 3:4417217b4f66 34 }
YusukeWakuta 6:a839ccbf7f9a 35
YusukeWakuta 3:4417217b4f66 36 //servo1.pulsewidth(calcPulse(a.read()*170));
YusukeWakuta 3:4417217b4f66 37 // servo2.pulsewidth(calcPulse(b.read()*170));
YusukeWakuta 3:4417217b4f66 38 //pc.printf("%f", a.read());
YusukeWakuta 6:a839ccbf7f9a 39
taurin 0:e052602db102 40
YusukeWakuta 6:a839ccbf7f9a 41 int main()
YusukeWakuta 6:a839ccbf7f9a 42 {
YusukeWakuta 6:a839ccbf7f9a 43 while(1) {
YusukeWakuta 6:a839ccbf7f9a 44 WriteServo(a,b);
taurin 2:7fcb4f970a02 45 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 46 }
taurin 0:e052602db102 47 }