2018F3RC3班 / Mbed 2 deprecated f3rc_3_auto_encode

f3rc 3班のエンコーダのプログラムです。

Dependencies:   EC2 Math mbed

main.cpp@1:0ff59878537a, 2018-09-03 (annotated)

Committer:
nobu0103
Date:
Mon Sep 03 05:21:06 2018 +0000
Revision:
1:0ff59878537a
Parent:
0:da183064372e
Child:
2:a682612dd8b7
f3rc 3????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nobu0103 0:da183064372e 1 #include "mbed.h"
nobu0103 0:da183064372e 2 #include "Math.h"
nobu0103 0:da183064372e 3 #include "Ec.h"
nobu0103 0:da183064372e 4 #include "SpeedController.h"
nobu0103 0:da183064372e 5 DigitalIn cylinder(p9,PullUp);
nobu0103 0:da183064372e 6 DigitalIn cylindert(p10,PullUp);
nobu0103 0:da183064372e 7 DigitalIn encoder1a(p11);
nobu0103 0:da183064372e 8 DigitalIn encoder1b(p12);
nobu0103 0:da183064372e 9 DigitalIn encoder2a(p13);
nobu0103 0:da183064372e 10 DigitalIn encoder2b(p14);
nobu0103 0:da183064372e 11 AnalogIn LBR1(p17,PullUp);
nobu0103 0:da183064372e 12 AnalogIn LBR2(p18,PullUp);
nobu0103 0:da183064372e 13 AnalogIn LBR3(p19,PullUp);
nobu0103 0:da183064372e 14 AnalogIn LBR4(p20,PullUp);
nobu0103 0:da183064372e 15 PwmOut servo12(p21);
nobu0103 0:da183064372e 16 PwmOut servo3(p22);
nobu0103 0:da183064372e 17 PwmOut motor1(p25);
nobu0103 0:da183064372e 18 PwmOut motor2(p26);
nobu0103 0:da183064372e 19 DigitalIn motor1cw(p27,PullUp);
nobu0103 0:da183064372e 20 DigitalIn motor2cw(p28,PullUp);
nobu0103 0:da183064372e 21
nobu0103 0:da183064372e 22 EC wheel_1 (p11,p12,/*z相*/,/*分解能*/,0.05)
nobu0103 0:da183064372e 23 EC wheel_2 (p13,p14,/*z相*/,/*分解能*/,0.05)
nobu0103 0:da183064372e 24 //SpeedControl (p11,p12,/*z相*/,/*分解能*/,0.05,,) pwmでないのでscは使えないかも
nobu0103 0:da183064372e 25 //SpeedControl (p13,p14,/*z相*/,/*分解能*/,0.05,,) pwmでないのでscは使えないかも
nobu0103 0:da183064372e 26
nobu0103 1:0ff59878537a 27 /*直進用*/
nobu0103 0:da183064372e 28 void straight(double t,int l) //t=時間 l=移動距離
nobu0103 0:da183064372e 29 {
nobu0103 0:da183064372e 30 mortor1.period_us(50);
nobu0103 0:da183064372e 31 mortor2.period_us(50);
nobu0103 1:0ff59878537a 32 double EC1_omega = wheel_1.getOmega(); //ec1の角速度
nobu0103 1:0ff59878537a 33 double EC2_omega = wheel_2.getOmega(); //ec2の角速度
nobu0103 1:0ff59878537a 34 double L_1 = EC1_omega*"ecの半径"*t;
nobu0103 1:0ff59878537a 35 double L_2 = EC2_omega*"ecの半径"*t;
nobu0103 1:0ff59878537a 36 while(L_1 <= l && L_2 <= l) {
nobu0103 1:0ff59878537a 37 motor1 = ;
nobu0103 1:0ff59878537a 38 motor2 = ;
nobu0103 1:0ff59878537a 39 motor1cw = ;
nobu0103 1:0ff59878537a 40 motor2cw = ;
nobu0103 1:0ff59878537a 41 }
nobu0103 1:0ff59878537a 42 }
nobu0103 1:0ff59878537a 43
nobu0103 1:0ff59878537a 44 /*後退用*/
nobu0103 1:0ff59878537a 45 void back(double t,int l) //t=時間 l=移動距離
nobu0103 1:0ff59878537a 46 {
nobu0103 1:0ff59878537a 47 mortor1.period_us(50);
nobu0103 1:0ff59878537a 48 mortor2.period_us(50);
nobu0103 0:da183064372e 49 double EC1_omega = wheel_1.getOmega();
nobu0103 0:da183064372e 50 double EC2_omega = wheel_2.getOmega();
nobu0103 0:da183064372e 51 double L_1 = EC1_omega*"ecの半径"*t;
nobu0103 0:da183064372e 52 double L_2 = EC2_omega*"ecの半径"*t;
nobu0103 0:da183064372e 53 while(L_1 <= l && L_2 <= l) {
nobu0103 0:da183064372e 54 motor1 = ;
nobu0103 0:da183064372e 55 motor2 = ;
nobu0103 0:da183064372e 56 motor1cw = ;
nobu0103 0:da183064372e 57 motor2cw = ;
nobu0103 0:da183064372e 58 }
nobu0103 0:da183064372e 59 }
nobu0103 0:da183064372e 60
nobu0103 0:da183064372e 61 /*右回転*/
nobu0103 0:da183064372e 62 void right(double t,int l) //t=時間 l=機体の中心を回転中心としたときのエンコーダの移動距離
nobu0103 0:da183064372e 63 {
nobu0103 0:da183064372e 64 mortor1.period_us(50);
nobu0103 0:da183064372e 65 mortor2.period_us(50);
nobu0103 0:da183064372e 66 double EC1_omega = wheel_1.getOmega();
nobu0103 0:da183064372e 67 double EC2_omega = wheel_2.getOmega();
nobu0103 0:da183064372e 68 double L_1 = EC1_omega*"ecの半径"*t;
nobu0103 0:da183064372e 69 double L_2 = EC2_omega*"ecの半径"*t;
nobu0103 0:da183064372e 70 while(L_1 <= l && L_2 <= -1*l) {
nobu0103 0:da183064372e 71 motor1 = ;
nobu0103 0:da183064372e 72 motor2 = ;
nobu0103 0:da183064372e 73 motor1cw = ;
nobu0103 0:da183064372e 74 motor2cw = ;
nobu0103 0:da183064372e 75 }
nobu0103 0:da183064372e 76 }
nobu0103 0:da183064372e 77
nobu0103 0:da183064372e 78 /*左回転*/
nobu0103 0:da183064372e 79 void left(double t,int l) //t=時間 l=l=機体の中心を回転中心としたときのエンコーダの移動距離
nobu0103 0:da183064372e 80 {
nobu0103 0:da183064372e 81 mortor1.period_us(50);
nobu0103 0:da183064372e 82 mortor2.period_us(50);
nobu0103 0:da183064372e 83 double EC1_omega = wheel_1.getOmega();
nobu0103 0:da183064372e 84 double EC2_omega = wheel_2.getOmega();
nobu0103 0:da183064372e 85 double L_1 = EC1_omega*"ecの半径"*t;
nobu0103 0:da183064372e 86 double L_2 = EC2_omega*"ecの半径"*t;
nobu0103 0:da183064372e 87 while(L_1 <= -1*l && L_2 <= l) {
nobu0103 0:da183064372e 88 motor1 = ;
nobu0103 0:da183064372e 89 motor2 = ;
nobu0103 0:da183064372e 90 motor1cw = ;
nobu0103 0:da183064372e 91 motor2cw = ;
nobu0103 0:da183064372e 92 }
nobu0103 0:da183064372e 93 }