勇紀 有岡 / Mbed 2 deprecated NHK2020-main

ver2

Dependencies:   uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P

manual/manual.cpp@2:820dcd23c8e3, 2019-12-11 (annotated)

Committer:
yuki0701
Date:
Wed Dec 11 04:59:36 2019 +0000
Revision:
2:820dcd23c8e3
Parent:
1:26fc1b2f1c42 
A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:b87fd8dd4322 1 #include "EC.h"
la00noix 0:b87fd8dd4322 2 #include "R1370P.h"
la00noix 0:b87fd8dd4322 3 #include "move4wheel.h"
la00noix 0:b87fd8dd4322 4 #include "mbed.h"
la00noix 0:b87fd8dd4322 5 #include "math.h"
la00noix 0:b87fd8dd4322 6 #include "PathFollowing.h"
la00noix 0:b87fd8dd4322 7 #include "movement.h"
la00noix 0:b87fd8dd4322 8 #include "manual.h"
la00noix 0:b87fd8dd4322 9 #include "can.h"
la00noix 0:b87fd8dd4322 10
la00noix 0:b87fd8dd4322 11 #define PI 3.141592
la00noix 0:b87fd8dd4322 12
la00noix 0:b87fd8dd4322 13 int id1_value[7]= {0};
la00noix 0:b87fd8dd4322 14
la00noix 0:b87fd8dd4322 15 //-----手動用の変数宣言--------------------------------------------------------------------------//
la00noix 0:b87fd8dd4322 16 int stick_theta; //ジョイスティックの角度
la00noix 0:b87fd8dd4322 17 int manual_xout,manual_yout; //フィールド座標系のx,y方向の速度
la00noix 0:b87fd8dd4322 18 int manual_realxout,manual_realyout; //機体座標系のx,y方向の速度
la00noix 0:b87fd8dd4322 19 int manual_rout; //旋回速度
la00noix 0:b87fd8dd4322 20
la00noix 0:b87fd8dd4322 21 void CalManualOut(int v,int r_out) //vはθ方向の速度、r_outは旋回速度(正の値)
la00noix 0:b87fd8dd4322 22 //PS3ジョイスティックの x=127.5 かつ y>127.5 の直線を0°としてθをとる
la00noix 0:b87fd8dd4322 23 {
la00noix 0:b87fd8dd4322 24 stick_theta = (short)((id1_value[1]<<8) | id1_value[2]);
la00noix 0:b87fd8dd4322 25 //debug_printf("stick = %d\n\r",stick_theta);
la00noix 0:b87fd8dd4322 26
la00noix 0:b87fd8dd4322 27 //ジョイスティック方向の速度をフィールド座標系の速度に変換
la00noix 0:b87fd8dd4322 28 manual_xout = v * sin(PI*stick_theta/180);
la00noix 0:b87fd8dd4322 29 manual_yout = -v * cos(PI*stick_theta/180);
la00noix 0:b87fd8dd4322 30
la00noix 0:b87fd8dd4322 31 //フィールド座標系の速度を機体座標系の速度に変換
la00noix 0:b87fd8dd4322 32 manual_realxout = manual_xout * cos(PI*now_angle/180) + manual_yout * sin(PI*now_angle/180);
la00noix 0:b87fd8dd4322 33 manual_realyout = -manual_xout * sin(PI*now_angle/180) + manual_yout * cos(PI*now_angle/180);
la00noix 0:b87fd8dd4322 34
la00noix 0:b87fd8dd4322 35 if(id1_value[4] == 1) { //旋回速度を代入
la00noix 0:b87fd8dd4322 36 manual_rout = 0;
la00noix 0:b87fd8dd4322 37 } else if(id1_value[4] == 2) {
la00noix 0:b87fd8dd4322 38 manual_rout = r_out;
la00noix 0:b87fd8dd4322 39 } else if(id1_value[4] == 3) {
la00noix 0:b87fd8dd4322 40 manual_rout = -r_out;
la00noix 0:b87fd8dd4322 41 }
la00noix 0:b87fd8dd4322 42
la00noix 0:b87fd8dd4322 43
la00noix 0:b87fd8dd4322 44 }
la00noix 0:b87fd8dd4322 45
la00noix 0:b87fd8dd4322 46 void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r)
la00noix 0:b87fd8dd4322 47 {
la00noix 0:b87fd8dd4322 48
la00noix 0:b87fd8dd4322 49 calc_gyro();
la00noix 0:b87fd8dd4322 50
la00noix 0:b87fd8dd4322 51 if(id1_value[3]==1) { //BOTTONR1押したら減速
la00noix 0:b87fd8dd4322 52 CalManualOut(slow_v,slow_r);
la00noix 0:b87fd8dd4322 53 } else {
la00noix 0:b87fd8dd4322 54 CalManualOut(fast_v,fast_r);
la00noix 0:b87fd8dd4322 55 }
la00noix 0:b87fd8dd4322 56
la00noix 0:b87fd8dd4322 57 if(id1_value[5] == 1) { //ニュートラルポジションなら出力0
la00noix 0:b87fd8dd4322 58 output(0,0,0,0);
la00noix 0:b87fd8dd4322 59 base(manual_rout,manual_rout,manual_rout,manual_rout,4095);
la00noix 0:b87fd8dd4322 60 } else {
la00noix 0:b87fd8dd4322 61 CalMotorOut(manual_realxout,manual_realyout,0);
la00noix 0:b87fd8dd4322 62 base(GetMotorOut(0)+manual_rout,GetMotorOut(1)+manual_rout,GetMotorOut(2)+manual_rout,GetMotorOut(3)+manual_rout,4095);
la00noix 0:b87fd8dd4322 63 }
la00noix 0:b87fd8dd4322 64
yuki0701 1:26fc1b2f1c42 65 //MaxonControl(m1,m2,m3,m4);
yuki0701 1:26fc1b2f1c42 66 // debug_printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle);
yuki0701 1:26fc1b2f1c42 67 printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle);
la00noix 0:b87fd8dd4322 68 }