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 kusano kiyoshige

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }