sampleProgram
Dependencies: QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo
Fork of 17robo_fuzi by
main.cpp
00001 //index_include 00002 #include "mbed.h" 00003 #include "math.h" 00004 #include "bit_test.h" 00005 #include "rs422_put.h" 00006 #include "sbdbt.h" 00007 #include "mecanum.h" 00008 #include "bno055_lib.h" 00009 #include "bno055_use.h" 00010 #include "pid.h" 00011 #include "limit.h" 00012 #include "accelerator.h" 00013 #include "pid_encoder.h" 00014 #include "cyclic_var.h" 00015 #include "cyclic_io.h" 00016 #include "cylinder.h" 00017 #include "event_var.h" 00018 00019 //index_define 00020 #define SBDBT_TX p13 00021 #define SBDBT_RX p14 00022 #define SBDBT_STO p12 00023 #define RS422_TX p28 00024 #define RS422_RX p27 00025 #define CONTROL_INDICATOR p11 00026 00027 #define SHOULDER_LEFT_HIGH p6 00028 #define SHOULDER_LEFT_LOW p7 00029 #define SHOULDER_RIGHT_HIGH p8 00030 #define SHOULDER_RIGHT_LOW p9 00031 00032 #define GROUND_ENCORDER_X_A p10 00033 #define GROUND_ENCORDER_X_B p11 00034 #define GROUND_ENCORDER_Y_A p12 00035 #define GROUND_ENCORDER_Y_B p13 00036 #define GUN_ENCORDER_A p14 00037 #define GUN_ENCORDER_B p15 00038 00039 #define GUN_ENCORDER_A p7 00040 #define GUN_ENCORDER_B p8 00041 00042 #define nucleo_num 4 00043 #define n0_id 0 00044 #define n1_id 1 00045 #define n2_id 2 00046 #define n3_id 3 00047 00048 //index_namespace_const_variable 00049 namespace BAUD { 00050 const int PC = 460800; 00051 const int SBDBT = 115200; 00052 const int RS422 = 115200; 00053 } 00054 00055 namespace CYCLE { 00056 const float OUTPUT = 0.015; 00057 } 00058 00059 namespace MECANUM { 00060 const float ACCELERATION = 15; 00061 } 00062 00063 namespace POWER { 00064 const float MECANUM = 1.0; 00065 const float SWORD = 1.0; 00066 const float SHOT = 1.0; 00067 const float RELOAD = 1.0; 00068 } 00069 00070 //index_namespace_variable 00071 namespace yaw{ 00072 float now; 00073 float target; 00074 } 00075 00076 //index_Function 00077 void setup(); 00078 void output(); 00079 void motorCalculation(); 00080 void shoulderCalcration(); 00081 void controllerEvent(); 00082 00083 //index_setupPin 00084 DigitalOut led1(LED1); 00085 DigitalOut led2(LED2); 00086 DigitalOut led3(LED3); 00087 DigitalOut led4(LED4); 00088 Serial pc(USBTX,USBRX); 00089 Rs422 rs422(RS422_TX, RS422_RX); 00090 Sbdbt sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO); 00091 Ticker outputTimer; 00092 Mecanum mecanum; 00093 Bno055 bno055; 00094 PositionPid yawPid; 00095 Accelerator v1; 00096 Accelerator v2; 00097 Accelerator v3; 00098 Accelerator v4; 00099 CyclicVar swordSwingCyclic; 00100 DigitalOut controlIndigator(CONTROL_INDICATOR); 00101 00102 //全国大会用追記 00103 Cylinder shoulderLeft(SHOULDER_LEFT_HIGH,SHOULDER_LEFT_LOW); 00104 Cylinder shoulderRight(SHOULDER_RIGHT_HIGH,SHOULDER_RIGHT_LOW); 00105 eventVar eventShikaku; 00106 eventVar eventSankaku; 00107 00108 00109 int main() 00110 { 00111 setup(); 00112 while(1) { 00113 //pc.printf("cylinder pos : %f\r\n",shotEncoder.deg()); 00114 //pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall()); 00115 } 00116 } 00117 00118 void shoulderCalclation(){ 00119 shoulderLeft.cyclic(eventShikaku.getRise()); 00120 shoulderRight.cyclic(eventSankaku.getRise()); 00121 } 00122 00123 void controllerEvent(){ 00124 eventShikaku.input(sbdbt.shikaku); 00125 eventSankaku.input(sbdbt.sankaku); 00126 } 00127 00128 void setup() 00129 { 00130 wait(1.0); 00131 bno055.begin(); 00132 wait(1.0); 00133 bno055.firstRead(); 00134 pc.baud(BAUD::PC); 00135 sbdbt.begin(BAUD::SBDBT); 00136 rs422.begin(BAUD::RS422); 00137 outputTimer.attach(&output, CYCLE::OUTPUT); 00138 mecanum.setupdeg(bno055.getYawRad()); //基盤が前後逆の場合+180 00139 v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); 00140 v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); 00141 v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); 00142 v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT); 00143 } 00144 00145 //index_mecanum 00146 void motorCalculation(){ 00147 static float out_right_x; 00148 yaw::now = bno055.getYawRad(); 00149 yaw::target = yaw::now; 00150 yawPid.calculate(yaw::target, yaw::now); 00151 00152 //後進時に方向が逆転するため 00153 if(sbdbt.right_y < 0.0){ 00154 out_right_x = -1.0*sbdbt.right_x; 00155 }else{ 00156 out_right_x = sbdbt.right_x; 00157 } 00158 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, bno055.getYawRad()); 00159 // pc.printf("%f\t data %f\t %f\t %f\t %f\t\r\n", bno055.getYawRad(), sbdbt.left_x, sbdbt.left_y, mecanum.VX(), mecanum.VY()); 00160 } 00161 00162 00163 //index_output 00164 void output() 00165 { 00166 //全国大会用追記 00167 shoulderCalclation(); 00168 controllerEvent(); 00169 00170 motorCalculation(); 00171 controlIndigator = sbdbt.get_pairingState(); 00172 //led1 = sbdbt.get_pairingState(); 00173 if(sbdbt.left)bno055.yaw_origin(); 00174 00175 static int counter; 00176 int id[nucleo_num] = {n0_id, n1_id, n2_id, n3_id}; 00177 00178 //sbdbtがpairingしている場合のみ動作 00179 if(sbdbt.get_pairingState()) { 00180 switch (counter) { 00181 case 0: 00182 rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*POWER::MECANUM)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*POWER::MECANUM)+(sbdbt.right_y*0.8)),1.0,-1.0))); 00183 counter++; 00184 break; 00185 case 1: 00186 //.duty(<cal>*<powerControle>+(<motorBoost>*0.5)) 00187 rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*POWER::MECANUM)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*POWER::MECANUM)-(sbdbt.right_y*0.8)),1.0,-1.0))); 00188 counter ++; 00189 break; 00190 case 2: 00191 rs422.put(id[counter], limitf(0.0*POWER::SHOT,1.0,-1.0), limitf(0.0*POWER::RELOAD,1.0,-1.0)); 00192 counter ++; 00193 break; 00194 case 3: 00195 rs422.put(id[counter], limitf(0.0*POWER::SWORD,1.0,-1.0), limitf(0.0,1.0,-1.0)); 00196 counter ++; 00197 break; 00198 default: 00199 break; 00200 } 00201 }else{ 00202 switch (counter) { 00203 case 0: 00204 rs422.put(id[counter],0.0,0.0); 00205 counter++; 00206 break; 00207 case 1: 00208 rs422.put(id[counter],0.0,0.0); 00209 counter ++; 00210 break; 00211 case 2: 00212 rs422.put(id[counter],0.0,0.0); 00213 counter ++; 00214 break; 00215 case 3: 00216 rs422.put(id[counter],0.0,0.0); 00217 counter ++; 00218 break; 00219 default: 00220 break; 00221 } 00222 } 00223 }
Generated on Mon Aug 8 2022 20:27:09 by
1.7.2
