自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
main.cpp@8:f2407caf81ba, 2022-03-05 (annotated)
- Committer:
- kazumayamanaka
- Date:
- Sat Mar 05 01:00:42 2022 +0000
- Revision:
- 8:f2407caf81ba
- Parent:
- 7:a0b9b6e71e41
program of motor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 0:a1238c4cd105 | 1 | #include "mbed.h" |
kazumayamanaka | 0:a1238c4cd105 | 2 | #include "rotary_inc.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 3 | #include "PIDco.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 4 | #include "TARGETco.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 5 | #include "DUALSHOCKco.hpp" |
kazumayamanaka | 2:e189d55ef292 | 6 | #include "Odmetry.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 7 | #include "sbdbt.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 8 | #include "BNO055.h" |
kazumayamanaka | 8:f2407caf81ba | 9 | #include "AUTOmatics.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 10 | #define SDA D3 |
kazumayamanaka | 0:a1238c4cd105 | 11 | #define SCL D6 |
kazumayamanaka | 0:a1238c4cd105 | 12 | #define PI 3.1415926535897 |
kazumayamanaka | 8:f2407caf81ba | 13 | #define NORMALMODE true |
kazumayamanaka | 8:f2407caf81ba | 14 | #define ROUNDMODE false |
kazumayamanaka | 0:a1238c4cd105 | 15 | |
kazumayamanaka | 0:a1238c4cd105 | 16 | Serial pc(USBTX,USBRX); |
kazumayamanaka | 0:a1238c4cd105 | 17 | |
kazumayamanaka | 0:a1238c4cd105 | 18 | RotaryInc data_1(PA_14,PA_15,0); |
kazumayamanaka | 0:a1238c4cd105 | 19 | RotaryInc data_2(PA_12,PC_5,0); |
kazumayamanaka | 0:a1238c4cd105 | 20 | RotaryInc data_3(PC_0,PC_1,0); |
kazumayamanaka | 0:a1238c4cd105 | 21 | RotaryInc data_4(PC_2,PC_3,0); |
kazumayamanaka | 0:a1238c4cd105 | 22 | |
kazumayamanaka | 2:e189d55ef292 | 23 | RotaryInc MW_1(PA_13,PC_4,0); |
kazumayamanaka | 2:e189d55ef292 | 24 | RotaryInc MW_2(PC_10,PC_11,0); |
kazumayamanaka | 2:e189d55ef292 | 25 | RotaryInc MW_3(PA_7,PA_6,0); |
kazumayamanaka | 2:e189d55ef292 | 26 | RotaryInc MW_4(PA_8,PA_9,0); |
kazumayamanaka | 2:e189d55ef292 | 27 | |
kazumayamanaka | 0:a1238c4cd105 | 28 | PIDco pid_1; |
kazumayamanaka | 0:a1238c4cd105 | 29 | PIDco pid_2; |
kazumayamanaka | 0:a1238c4cd105 | 30 | PIDco pid_3; |
kazumayamanaka | 0:a1238c4cd105 | 31 | PIDco pid_4; |
kazumayamanaka | 0:a1238c4cd105 | 32 | |
kazumayamanaka | 0:a1238c4cd105 | 33 | TARGETco TG; |
kazumayamanaka | 0:a1238c4cd105 | 34 | BNO055 bno(SDA,SCL); |
kazumayamanaka | 0:a1238c4cd105 | 35 | DUALSHOCKco DS; |
kazumayamanaka | 2:e189d55ef292 | 36 | Odmetry odmetry; |
kazumayamanaka | 0:a1238c4cd105 | 37 | sbdbt sb(PA_0,PA_1); |
kazumayamanaka | 0:a1238c4cd105 | 38 | |
kazumayamanaka | 0:a1238c4cd105 | 39 | Timer Time; |
kazumayamanaka | 7:a0b9b6e71e41 | 40 | bool mode=false; |
kazumayamanaka | 0:a1238c4cd105 | 41 | double timer; |
kazumayamanaka | 0:a1238c4cd105 | 42 | double theta; |
kazumayamanaka | 7:a0b9b6e71e41 | 43 | double automaticsX(double PosX); |
kazumayamanaka | 7:a0b9b6e71e41 | 44 | double automaticsY(double PosY); |
kazumayamanaka | 7:a0b9b6e71e41 | 45 | double automaticsTHETA(double THETA); |
kazumayamanaka | 7:a0b9b6e71e41 | 46 | void automatics(); |
kazumayamanaka | 8:f2407caf81ba | 47 | void Testautomatics(); |
kazumayamanaka | 8:f2407caf81ba | 48 | void diff(double PosX,double PosY,double PosT); |
kazumayamanaka | 8:f2407caf81ba | 49 | double diffX = 0,diffY = 0,diffT = 0; |
kazumayamanaka | 8:f2407caf81ba | 50 | double targetX=500,targetY=100,targetT=PI / 4; |
kazumayamanaka | 8:f2407caf81ba | 51 | bool set = true; |
kazumayamanaka | 0:a1238c4cd105 | 52 | |
kazumayamanaka | 0:a1238c4cd105 | 53 | int main(){ |
kazumayamanaka | 0:a1238c4cd105 | 54 | Time.start(); |
kazumayamanaka | 7:a0b9b6e71e41 | 55 | bno.reset(); |
kazumayamanaka | 2:e189d55ef292 | 56 | |
kazumayamanaka | 0:a1238c4cd105 | 57 | while(1){ |
kazumayamanaka | 0:a1238c4cd105 | 58 | timer = Time.read_us(); |
kazumayamanaka | 0:a1238c4cd105 | 59 | bno.setmode(OPERATION_MODE_IMUPLUS); |
kazumayamanaka | 0:a1238c4cd105 | 60 | bno.get_angles(); |
kazumayamanaka | 1:ea880e226e5a | 61 | |
kazumayamanaka | 7:a0b9b6e71e41 | 62 | if(sb.circle()!=0){ |
kazumayamanaka | 7:a0b9b6e71e41 | 63 | mode = true; |
kazumayamanaka | 7:a0b9b6e71e41 | 64 | } |
kazumayamanaka | 7:a0b9b6e71e41 | 65 | if(sb.square()!=0){ |
kazumayamanaka | 7:a0b9b6e71e41 | 66 | mode = false; |
kazumayamanaka | 7:a0b9b6e71e41 | 67 | } |
kazumayamanaka | 7:a0b9b6e71e41 | 68 | |
kazumayamanaka | 1:ea880e226e5a | 69 | theta = bno.euler.yaw * (PI / 180); |
kazumayamanaka | 1:ea880e226e5a | 70 | |
kazumayamanaka | 0:a1238c4cd105 | 71 | if(theta > PI){ |
kazumayamanaka | 1:ea880e226e5a | 72 | theta = -(2 * PI - theta); |
kazumayamanaka | 0:a1238c4cd105 | 73 | } |
kazumayamanaka | 0:a1238c4cd105 | 74 | |
kazumayamanaka | 2:e189d55ef292 | 75 | theta = theta - PI / 4; |
kazumayamanaka | 2:e189d55ef292 | 76 | |
kazumayamanaka | 7:a0b9b6e71e41 | 77 | if(mode == false){ |
kazumayamanaka | 7:a0b9b6e71e41 | 78 | DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An()); |
kazumayamanaka | 0:a1238c4cd105 | 79 | |
kazumayamanaka | 7:a0b9b6e71e41 | 80 | if(DS.cal_input() == true){ |
kazumayamanaka | 7:a0b9b6e71e41 | 81 | TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); |
kazumayamanaka | 7:a0b9b6e71e41 | 82 | } |
kazumayamanaka | 7:a0b9b6e71e41 | 83 | else{ |
kazumayamanaka | 7:a0b9b6e71e41 | 84 | TG.pass_target(DS.obt_X()); |
kazumayamanaka | 7:a0b9b6e71e41 | 85 | } |
kazumayamanaka | 0:a1238c4cd105 | 86 | } |
kazumayamanaka | 0:a1238c4cd105 | 87 | else{ |
kazumayamanaka | 7:a0b9b6e71e41 | 88 | automatics(); |
kazumayamanaka | 8:f2407caf81ba | 89 | pc.printf("%d ",set); |
kazumayamanaka | 0:a1238c4cd105 | 90 | } |
kazumayamanaka | 0:a1238c4cd105 | 91 | |
kazumayamanaka | 0:a1238c4cd105 | 92 | pid_1.pass_val(data_1.get(),TG.obt_target1()); |
kazumayamanaka | 0:a1238c4cd105 | 93 | pid_2.pass_val(data_2.get(),TG.obt_target2()); |
kazumayamanaka | 0:a1238c4cd105 | 94 | pid_3.pass_val(data_3.get(),TG.obt_target3()); |
kazumayamanaka | 0:a1238c4cd105 | 95 | pid_4.pass_val(data_4.get(),TG.obt_target4()); |
kazumayamanaka | 0:a1238c4cd105 | 96 | |
kazumayamanaka | 1:ea880e226e5a | 97 | pid_1.wheel_ctl(PC_9,PC_8); |
kazumayamanaka | 0:a1238c4cd105 | 98 | pid_2.wheel_ctl(PB_14,PB_13); |
kazumayamanaka | 0:a1238c4cd105 | 99 | pid_3.wheel_ctl(PB_5,PB_4); |
kazumayamanaka | 0:a1238c4cd105 | 100 | pid_4.wheel_ctl(PB_7,PB_6); |
kazumayamanaka | 0:a1238c4cd105 | 101 | |
kazumayamanaka | 2:e189d55ef292 | 102 | odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get()); |
kazumayamanaka | 2:e189d55ef292 | 103 | odmetry.pass_angle(theta); |
kazumayamanaka | 8:f2407caf81ba | 104 | //odmetry.print_Pos(); |
kazumayamanaka | 8:f2407caf81ba | 105 | pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd()); |
kazumayamanaka | 0:a1238c4cd105 | 106 | while(Time.read_us() - timer <= 50 * 1000); |
kazumayamanaka | 0:a1238c4cd105 | 107 | } |
kazumayamanaka | 0:a1238c4cd105 | 108 | } |
kazumayamanaka | 0:a1238c4cd105 | 109 | |
kazumayamanaka | 8:f2407caf81ba | 110 | |
kazumayamanaka | 8:f2407caf81ba | 111 | double automaticsX(){ |
kazumayamanaka | 8:f2407caf81ba | 112 | if(targetX > 0){ |
kazumayamanaka | 8:f2407caf81ba | 113 | return diffX * 500 / targetX; |
kazumayamanaka | 7:a0b9b6e71e41 | 114 | } |
kazumayamanaka | 7:a0b9b6e71e41 | 115 | else{ |
kazumayamanaka | 8:f2407caf81ba | 116 | return -diffX * 500 / targetX; |
kazumayamanaka | 7:a0b9b6e71e41 | 117 | } |
kazumayamanaka | 7:a0b9b6e71e41 | 118 | } |
kazumayamanaka | 8:f2407caf81ba | 119 | double automaticsY(){ |
kazumayamanaka | 8:f2407caf81ba | 120 | if(targetY > 0){ |
kazumayamanaka | 8:f2407caf81ba | 121 | return -diffY * 500 / targetY; |
kazumayamanaka | 7:a0b9b6e71e41 | 122 | } |
kazumayamanaka | 7:a0b9b6e71e41 | 123 | else{ |
kazumayamanaka | 8:f2407caf81ba | 124 | return diffY * 500 / targetY; |
kazumayamanaka | 7:a0b9b6e71e41 | 125 | } |
kazumayamanaka | 7:a0b9b6e71e41 | 126 | } |
kazumayamanaka | 8:f2407caf81ba | 127 | double automaticsTHETA(){ |
kazumayamanaka | 8:f2407caf81ba | 128 | if(targetT > 0)return -diffT * 500 / targetT; |
kazumayamanaka | 8:f2407caf81ba | 129 | else return diffT * 500 / targetT; |
kazumayamanaka | 7:a0b9b6e71e41 | 130 | } |
kazumayamanaka | 7:a0b9b6e71e41 | 131 | void automatics(){ |
kazumayamanaka | 8:f2407caf81ba | 132 | diff(targetX,targetY,targetT); |
kazumayamanaka | 8:f2407caf81ba | 133 | |
kazumayamanaka | 8:f2407caf81ba | 134 | if((diffX > 1)||(diffY > 1)){ |
kazumayamanaka | 8:f2407caf81ba | 135 | TG.pass_val(automaticsY(),automaticsX(),theta); |
kazumayamanaka | 8:f2407caf81ba | 136 | set = true; |
kazumayamanaka | 7:a0b9b6e71e41 | 137 | } |
kazumayamanaka | 8:f2407caf81ba | 138 | else set = false; |
kazumayamanaka | 8:f2407caf81ba | 139 | |
kazumayamanaka | 8:f2407caf81ba | 140 | if(set == false)TG.pass_target(automaticsTHETA()); |
kazumayamanaka | 8:f2407caf81ba | 141 | |
kazumayamanaka | 8:f2407caf81ba | 142 | printf(" %lf %lf ",diffX,diffY); |
kazumayamanaka | 7:a0b9b6e71e41 | 143 | } |
kazumayamanaka | 8:f2407caf81ba | 144 | |
kazumayamanaka | 8:f2407caf81ba | 145 | void diff(double PosX,double PosY,double PosT){ |
kazumayamanaka | 8:f2407caf81ba | 146 | diffX = PosX - odmetry.obt_CurrentPosX(); |
kazumayamanaka | 8:f2407caf81ba | 147 | diffY = PosY - odmetry.obt_CurrentPosY(); |
kazumayamanaka | 8:f2407caf81ba | 148 | diffT = PosT - odmetry.obt_CurrentTheta(); |
kazumayamanaka | 8:f2407caf81ba | 149 | } |
kazumayamanaka | 8:f2407caf81ba | 150 | |
kazumayamanaka | 7:a0b9b6e71e41 | 151 | |
kazumayamanaka | 7:a0b9b6e71e41 | 152 | |
kazumayamanaka | 0:a1238c4cd105 | 153 | //BNOのピン(PB_3,PB_10) |
kazumayamanaka | 0:a1238c4cd105 | 154 | |
kazumayamanaka | 0:a1238c4cd105 | 155 | |
kazumayamanaka | 0:a1238c4cd105 | 156 |