春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_1026

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   CruizCore_R1370P EC_speedcontrol_1010 enc_1multi mbed

Fork of harurobo1006 by 春ロボ1班(元F3RC4班+)

main.cpp@0:b811f40948e2, 2018-10-06 (annotated)

Committer:
aoikoizumi
Date:
Sat Oct 06 02:30:36 2018 +0000
Revision:
0:b811f40948e2
Child:
1:2189d6238ac7
?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aoikoizumi 0:b811f40948e2 1 #include "mbed.h"
aoikoizumi 0:b811f40948e2 2 #include "SpeedController.h"
aoikoizumi 0:b811f40948e2 3 #include "EC.h"
aoikoizumi 0:b811f40948e2 4 #include "R1370P.h"
aoikoizumi 0:b811f40948e2 5 #include "enc_1multi.h"
aoikoizumi 0:b811f40948e2 6 #include "math.h"
aoikoizumi 0:b811f40948e2 7 #define BASIC_SPEED 24 //モーターはこの角速度で駆動させる
aoikoizumi 0:b811f40948e2 8 #define PI 3.141592
aoikoizumi 0:b811f40948e2 9
aoikoizumi 0:b811f40948e2 10
aoikoizumi 0:b811f40948e2 11 SpeedControl Motor_RF(NC,NC,NC,500,0.05,NC,NC);
aoikoizumi 0:b811f40948e2 12 SpeedControl Motor_LF(NC,NC,NC,500,0.05,NC,NC);
aoikoizumi 0:b811f40948e2 13 SpeedControl Motor_LB(NC,NC,NC,500,0.05,NC,NC);
aoikoizumi 0:b811f40948e2 14 SpeedControl Motor_RB(NC,NC,NC,500,0.05,NC,NC);//モーター定義
aoikoizumi 0:b811f40948e2 15
aoikoizumi 0:b811f40948e2 16 Ec EC1(NC,NC,NC,300,0.05);
aoikoizumi 0:b811f40948e2 17 Ec EC2(NC,NC,NC,300,0.05);//エンコーダ定義
aoikoizumi 0:b811f40948e2 18
aoikoizumi 0:b811f40948e2 19 Ticker motor_tick; //角速度計算用ticker
aoikoizumi 0:b811f40948e2 20 Ticker ticker;//for enc
aoikoizumi 0:b811f40948e2 21
aoikoizumi 0:b811f40948e2 22 Serial pc(USBTX, USBRX); // tx, rx //PC USB
aoikoizumi 0:b811f40948e2 23 R1370P gyro(PC_6,PC_7);
aoikoizumi 0:b811f40948e2 24
aoikoizumi 0:b811f40948e2 25 void calOmega() //角速度計算関数
aoikoizumi 0:b811f40948e2 26 {
aoikoizumi 0:b811f40948e2 27 Motor_RF.CalOmega();
aoikoizumi 0:b811f40948e2 28 Motor_LF.CalOmega();
aoikoizumi 0:b811f40948e2 29 Motor_LB.CalOmega();
aoikoizumi 0:b811f40948e2 30 Motor_RB.CalOmega();
aoikoizumi 0:b811f40948e2 31 EC1.CalOmega();
aoikoizumi 0:b811f40948e2 32 EC2.CalOmega();
aoikoizumi 0:b811f40948e2 33 }
aoikoizumi 0:b811f40948e2 34 double new_dist1=0,new_dist2=0;
aoikoizumi 0:b811f40948e2 35 double old_dist1=0,old_dist2=0;
aoikoizumi 0:b811f40948e2 36 double d_dist1=0,d_dist2=0;//座標計算用関数
aoikoizumi 0:b811f40948e2 37 double now_x;//現在地X座標
aoikoizumi 0:b811f40948e2 38 double now_y;//現在地Y座標
aoikoizumi 0:b811f40948e2 39 double now_angle; //現在角度
aoikoizumi 0:b811f40948e2 40
aoikoizumi 0:b811f40948e2 41 double start_x=0;
aoikoizumi 0:b811f40948e2 42 double start_y=0;
aoikoizumi 0:b811f40948e2 43
aoikoizumi 0:b811f40948e2 44 double target_RF=0,target_LF=0,target_LB=0,target_RB=0;//目標速度
aoikoizumi 0:b811f40948e2 45 Timer t;
aoikoizumi 0:b811f40948e2 46 int i=0;//い　つ　も　の
aoikoizumi 0:b811f40948e2 47
aoikoizumi 0:b811f40948e2 48
aoikoizumi 0:b811f40948e2 49
aoikoizumi 0:b811f40948e2 50 void idou(double iRF,double iLF,double iLB,double iRB)//各モーターの動作明瞭化
aoikoizumi 0:b811f40948e2 51 {
aoikoizumi 0:b811f40948e2 52 target_RF=BASIC_SPEED*iRF;
aoikoizumi 0:b811f40948e2 53 target_LF=BASIC_SPEED*iLF;
aoikoizumi 0:b811f40948e2 54 target_LB=BASIC_SPEED*iLB;
aoikoizumi 0:b811f40948e2 55 target_RB=BASIC_SPEED*iRB;
aoikoizumi 0:b811f40948e2 56 }
aoikoizumi 0:b811f40948e2 57
aoikoizumi 0:b811f40948e2 58
aoikoizumi 0:b811f40948e2 59 int main()
aoikoizumi 0:b811f40948e2 60 {
aoikoizumi 0:b811f40948e2 61 gyro.initialize(); //main関数の最初に一度だけ実行
aoikoizumi 0:b811f40948e2 62 gyro.acc_offset(); //やってもやらなくてもいい
aoikoizumi 0:b811f40948e2 63 printf("start\r\n");
aoikoizumi 0:b811f40948e2 64
aoikoizumi 0:b811f40948e2 65 motor_tick.attach(&calOmega,0.05);
aoikoizumi 0:b811f40948e2 66 Motor_RF.setDOconstant(30);
aoikoizumi 0:b811f40948e2 67 Motor_LF.setDOconstant(30);//c
aoikoizumi 0:b811f40948e2 68 Motor_LB.setDOconstant(30);
aoikoizumi 0:b811f40948e2 69 Motor_RB.setDOconstant(30);//c
aoikoizumi 0:b811f40948e2 70
aoikoizumi 0:b811f40948e2 71 Motor_RF.setPDparam(0,0);
aoikoizumi 0:b811f40948e2 72 Motor_LF.setPDparam(0,0);//pd
aoikoizumi 0:b811f40948e2 73 Motor_LB.setPDparam(0,0);
aoikoizumi 0:b811f40948e2 74 Motor_RB.setPDparam(0,0);//pd
aoikoizumi 0:b811f40948e2 75
aoikoizumi 0:b811f40948e2 76
aoikoizumi 0:b811f40948e2 77 EC1.setDiameter_mm(48);
aoikoizumi 0:b811f40948e2 78 EC2.setDiameter_mm(48);//測定輪半径
aoikoizumi 0:b811f40948e2 79 double getDistance_mm();
aoikoizumi 0:b811f40948e2 80
aoikoizumi 0:b811f40948e2 81 void reset();
aoikoizumi 0:b811f40948e2 82 EC1.reset();
aoikoizumi 0:b811f40948e2 83
aoikoizumi 0:b811f40948e2 84 now_x=start_x;
aoikoizumi 0:b811f40948e2 85 now_y=start_y;
aoikoizumi 0:b811f40948e2 86 while(1) {
aoikoizumi 0:b811f40948e2 87 now_angle=gyro.getAngle();
aoikoizumi 0:b811f40948e2 88
aoikoizumi 0:b811f40948e2 89 new_dist1=EC1.getDistance_mm();
aoikoizumi 0:b811f40948e2 90 new_dist2=EC2.getDistance_mm();
aoikoizumi 0:b811f40948e2 91 d_dist1=new_dist1-old_dist1;
aoikoizumi 0:b811f40948e2 92 d_dist2=new_dist2-old_dist2;
aoikoizumi 0:b811f40948e2 93 old_dist1=new_dist1;
aoikoizumi 0:b811f40948e2 94 old_dist2=new_dist2;//微小時間当たりのエンコーダ読み込み
aoikoizumi 0:b811f40948e2 95
aoikoizumi 0:b811f40948e2 96
aoikoizumi 0:b811f40948e2 97 double d_x=d_dist2*sin(now_angle*3.1415926535/180)-d_dist1*sin(now_angle*3.1415926535/180);
aoikoizumi 0:b811f40948e2 98 double d_y=d_dist2*sin(now_angle*3.1415926535/180)+d_dist1*cos(now_angle*3.1415926535/180);//微小時間毎の座標変化
aoikoizumi 0:b811f40948e2 99
aoikoizumi 0:b811f40948e2 100
aoikoizumi 0:b811f40948e2 101 now_x=now_x+d_x;
aoikoizumi 0:b811f40948e2 102 now_y=now_y+d_y;//微小時間毎に座標に加算
aoikoizumi 0:b811f40948e2 103 }
aoikoizumi 0:b811f40948e2 104 }