kusano kiyoshige / Mbed 2 deprecated 17robo_tokyo_kaede

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

main.cpp@75:cd507886ee57, 2017-10-22 (annotated)

Committer:
echo_piyo
Date:
Sun Oct 22 11:18:08 2017 +0000
Revision:
75:cd507886ee57
Parent:
74:98bad67fde39
Child:
76:03475cafb326
(??)??????????????; SBDBT?eventVar?????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 56:a7bd860b85b6 1 //index_include
echo_piyo 0:bf96e953cdb8 2 #include "mbed.h"
echo_piyo 4:a6cc2f03e69b 3 #include "math.h"
echo_piyo 68:18cd96c13fa1 4 #include "bit_test.h"
echo_piyo 66:1664ee92539d 5 #include "rs422_put.h"
echo_piyo 0:bf96e953cdb8 6 #include "sbdbt.h"
echo_piyo 0:bf96e953cdb8 7 #include "mecanum.h"
echo_piyo 0:bf96e953cdb8 8 #include "bno055_lib.h"
echo_piyo 0:bf96e953cdb8 9 #include "bno055_use.h"
echo_piyo 0:bf96e953cdb8 10 #include "pid.h"
echo_piyo 0:bf96e953cdb8 11 #include "limit.h"
echo_piyo 4:a6cc2f03e69b 12 #include "accelerator.h"
echo_piyo 66:1664ee92539d 13 #include "pid_encoder.h"
echo_piyo 66:1664ee92539d 14 #include "cyclic_var.h"
echo_piyo 66:1664ee92539d 15 #include "cyclic_io.h"
echo_piyo 15:0fdf483769bf 16 #include "cylinder.h"
echo_piyo 66:1664ee92539d 17 #include "event_var.h"
echo_piyo 69:b2c8a0c5662c 18
echo_piyo 56:a7bd860b85b6 19 //index_define
echo_piyo 71:7d684ff43ddd 20 #define SBDBT_TX p13
echo_piyo 71:7d684ff43ddd 21 #define SBDBT_RX p14
echo_piyo 71:7d684ff43ddd 22 #define SBDBT_STO p12
echo_piyo 71:7d684ff43ddd 23 #define RS422_TX p28
echo_piyo 71:7d684ff43ddd 24 #define RS422_RX p27
echo_piyo 71:7d684ff43ddd 25 #define CONTROL_INDICATOR p11
echo_piyo 71:7d684ff43ddd 26
echo_piyo 75:cd507886ee57 27 #define SHOULDER_LEFT_HIGH p6
echo_piyo 75:cd507886ee57 28 #define SHOULDER_LEFT_LOW p7
echo_piyo 75:cd507886ee57 29 #define SHOULDER_RIGHT_HIGH p8
echo_piyo 75:cd507886ee57 30 #define SHOULDER_RIGHT_LOW p9
echo_piyo 72:7d6177047823 31
echo_piyo 75:cd507886ee57 32 #define GROUND_ENCORDER_X_A p10
echo_piyo 75:cd507886ee57 33 #define GROUND_ENCORDER_X_B p11
echo_piyo 75:cd507886ee57 34 #define GROUND_ENCORDER_Y_A p12
echo_piyo 75:cd507886ee57 35 #define GROUND_ENCORDER_Y_B p13
echo_piyo 72:7d6177047823 36
echo_piyo 74:98bad67fde39 37 #define GUN_ENCORDER_A p7
echo_piyo 74:98bad67fde39 38 #define GUN_ENCORDER_B p8
echo_piyo 74:98bad67fde39 39
echo_piyo 74:98bad67fde39 40 #define nucleo_num 4
echo_piyo 74:98bad67fde39 41 #define n0_id 0
echo_piyo 74:98bad67fde39 42 #define n1_id 1
echo_piyo 74:98bad67fde39 43 #define n2_id 2
echo_piyo 74:98bad67fde39 44 #define n3_id 3
echo_piyo 72:7d6177047823 45
echo_piyo 72:7d6177047823 46 //index_namespace_const_variable
echo_piyo 71:7d684ff43ddd 47 namespace BAUD {
echo_piyo 71:7d684ff43ddd 48 const int PC = 460800;
echo_piyo 71:7d684ff43ddd 49 const int SBDBT = 115200;
echo_piyo 71:7d684ff43ddd 50 const int RS422 = 115200;
echo_piyo 71:7d684ff43ddd 51 }
echo_piyo 71:7d684ff43ddd 52
echo_piyo 71:7d684ff43ddd 53 namespace CYCLE {
echo_piyo 71:7d684ff43ddd 54 const float OUTPUT = 0.015;
echo_piyo 71:7d684ff43ddd 55 }
echo_piyo 71:7d684ff43ddd 56
echo_piyo 71:7d684ff43ddd 57 namespace MECANUM {
echo_piyo 71:7d684ff43ddd 58 const float ACCELERATION = 15;
echo_piyo 71:7d684ff43ddd 59 }
echo_piyo 71:7d684ff43ddd 60
echo_piyo 72:7d6177047823 61 namespace POWER {
echo_piyo 72:7d6177047823 62 const float MECANUM = 1.0;
echo_piyo 72:7d6177047823 63 const float SWORD = 1.0;
echo_piyo 74:98bad67fde39 64 const float SHOT = 1.0;
echo_piyo 74:98bad67fde39 65 const float RELOAD = 1.0;
echo_piyo 72:7d6177047823 66 }
echo_piyo 71:7d684ff43ddd 67
echo_piyo 72:7d6177047823 68 //index_namespace_variable
echo_piyo 72:7d6177047823 69 namespace yaw{
echo_piyo 72:7d6177047823 70 float now;
echo_piyo 72:7d6177047823 71 float target;
echo_piyo 72:7d6177047823 72 }
echo_piyo 71:7d684ff43ddd 73
echo_piyo 72:7d6177047823 74 //index_Function
echo_piyo 72:7d6177047823 75 void setup();
echo_piyo 72:7d6177047823 76 void output();
echo_piyo 72:7d6177047823 77 void motorCalculation();
echo_piyo 57:68df771fd8a1 78
echo_piyo 56:a7bd860b85b6 79 //index_setupPin
echo_piyo 72:7d6177047823 80 DigitalOut led1(LED1);
echo_piyo 72:7d6177047823 81 DigitalOut led2(LED2);
echo_piyo 72:7d6177047823 82 DigitalOut led3(LED3);
echo_piyo 72:7d6177047823 83 DigitalOut led4(LED4);
echo_piyo 72:7d6177047823 84 Serial pc(USBTX,USBRX);
echo_piyo 72:7d6177047823 85 Rs422 rs422(RS422_TX, RS422_RX);
echo_piyo 72:7d6177047823 86 Sbdbt sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO);
echo_piyo 72:7d6177047823 87 Ticker outputTimer;
echo_piyo 72:7d6177047823 88 Mecanum mecanum;
echo_piyo 72:7d6177047823 89 Bno055 bno055;
echo_piyo 72:7d6177047823 90 PositionPid yawPid;
echo_piyo 66:1664ee92539d 91 Accelerator v1;
echo_piyo 66:1664ee92539d 92 Accelerator v2;
echo_piyo 66:1664ee92539d 93 Accelerator v3;
echo_piyo 66:1664ee92539d 94 Accelerator v4;
echo_piyo 72:7d6177047823 95 CyclicVar swordSwingCyclic;
echo_piyo 72:7d6177047823 96 DigitalOut controlIndigator(CONTROL_INDICATOR);
echo_piyo 67:b094d88583be 97
echo_piyo 75:cd507886ee57 98 //全国大会用追記
echo_piyo 75:cd507886ee57 99 Cylinder shoulderLeft(SHOULDER_LEFT_HIGH,SHOULDER_LEFT_LOW);
echo_piyo 75:cd507886ee57 100 Cylinder shoulderRight(SHOULDER_RIGHT_HIGH,SHOULDER_RIGHT_LOW);
echo_piyo 75:cd507886ee57 101 eventVar eventShikaku;
echo_piyo 75:cd507886ee57 102 eventVar eventSankaku;
echo_piyo 75:cd507886ee57 103
echo_piyo 0:bf96e953cdb8 104
echo_piyo 0:bf96e953cdb8 105 int main()
echo_piyo 0:bf96e953cdb8 106 {
echo_piyo 0:bf96e953cdb8 107 setup();
echo_piyo 2:d5b8f8e62923 108 while(1) {
echo_piyo 72:7d6177047823 109 //pc.printf("cylinder pos : %f\r\n",shotEncoder.deg());
echo_piyo 55:2dd2f161ebaf 110 //pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall());
echo_piyo 0:bf96e953cdb8 111 }
echo_piyo 0:bf96e953cdb8 112 }
echo_piyo 0:bf96e953cdb8 113
echo_piyo 75:cd507886ee57 114 void shoulder(){
echo_piyo 75:cd507886ee57 115 shoulderLeft.cyclic(eventShikaku.getRise());
echo_piyo 75:cd507886ee57 116 shoulderRight.cyclic(eventSankaku.getRise());
echo_piyo 75:cd507886ee57 117 }
echo_piyo 75:cd507886ee57 118
echo_piyo 75:cd507886ee57 119 void controllerEvent(){
echo_piyo 75:cd507886ee57 120 eventShikaku.input(sbdbt.shikaku);
echo_piyo 75:cd507886ee57 121 eventSankaku.input(sbdbt.sankaku);
echo_piyo 75:cd507886ee57 122 }
echo_piyo 75:cd507886ee57 123
echo_piyo 0:bf96e953cdb8 124 void setup()
echo_piyo 0:bf96e953cdb8 125 {
echo_piyo 72:7d6177047823 126 wait(1.0);
echo_piyo 65:5e4c2e5494ae 127 bno055.begin();
echo_piyo 40:2d6888448ab2 128 wait(1.0);
echo_piyo 65:5e4c2e5494ae 129 bno055.firstRead();
echo_piyo 71:7d684ff43ddd 130 pc.baud(BAUD::PC);
echo_piyo 71:7d684ff43ddd 131 sbdbt.begin(BAUD::SBDBT);
echo_piyo 71:7d684ff43ddd 132 rs422.begin(BAUD::RS422);
echo_piyo 72:7d6177047823 133 outputTimer.attach(&output, CYCLE::OUTPUT);
echo_piyo 72:7d6177047823 134 mecanum.setupdeg(bno055.getYawRad()); //基盤が前後逆の場合+180
echo_piyo 71:7d684ff43ddd 135 v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
echo_piyo 71:7d684ff43ddd 136 v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
echo_piyo 71:7d684ff43ddd 137 v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
echo_piyo 71:7d684ff43ddd 138 v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
echo_piyo 10:04f2a82cfd89 139 }
echo_piyo 10:04f2a82cfd89 140
echo_piyo 72:7d6177047823 141 //index_mecanum
echo_piyo 72:7d6177047823 142 void motorCalculation(){
echo_piyo 56:a7bd860b85b6 143 static float out_right_x;
echo_piyo 72:7d6177047823 144 yaw::now = bno055.getYawRad();
echo_piyo 72:7d6177047823 145 yaw::target = yaw::now;
echo_piyo 72:7d6177047823 146 yawPid.calculate(yaw::target, yaw::now);
echo_piyo 56:a7bd860b85b6 147
echo_piyo 56:a7bd860b85b6 148 //後進時に方向が逆転するため
echo_piyo 56:a7bd860b85b6 149 if(sbdbt.right_y < 0.0){
echo_piyo 56:a7bd860b85b6 150 out_right_x = -1.0*sbdbt.right_x;
echo_piyo 56:a7bd860b85b6 151 }else{
echo_piyo 56:a7bd860b85b6 152 out_right_x = sbdbt.right_x;
echo_piyo 56:a7bd860b85b6 153 }
echo_piyo 56:a7bd860b85b6 154 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, bno055.getYawRad());
echo_piyo 5:6efda58ff71b 155 // 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());
echo_piyo 35:5e1ad00f26fb 156 }
echo_piyo 72:7d6177047823 157
echo_piyo 74:98bad67fde39 158
echo_piyo 72:7d6177047823 159 //index_output
echo_piyo 35:5e1ad00f26fb 160 void output()
echo_piyo 35:5e1ad00f26fb 161 {
echo_piyo 72:7d6177047823 162 motorCalculation();
echo_piyo 72:7d6177047823 163 controlIndigator = sbdbt.get_pairingState();
echo_piyo 48:96b5f5ebdfb0 164 //led1 = sbdbt.get_pairingState();
echo_piyo 49:b041c815c063 165 if(sbdbt.left)bno055.yaw_origin();
echo_piyo 72:7d6177047823 166
echo_piyo 35:5e1ad00f26fb 167 static int counter;
echo_piyo 74:98bad67fde39 168 int id[nucleo_num] = {n0_id, n1_id, n2_id, n3_id};
echo_piyo 45:a32e8091901b 169
echo_piyo 45:a32e8091901b 170 //sbdbtがpairingしている場合のみ動作
echo_piyo 45:a32e8091901b 171 if(sbdbt.get_pairingState()) {
echo_piyo 45:a32e8091901b 172 switch (counter) {
echo_piyo 45:a32e8091901b 173 case 0:
echo_piyo 72:7d6177047823 174 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)));
echo_piyo 45:a32e8091901b 175 counter++;
echo_piyo 45:a32e8091901b 176 break;
echo_piyo 45:a32e8091901b 177 case 1:
echo_piyo 72:7d6177047823 178 //.duty(<cal>*<powerControle>+(<motorBoost>*0.5))
echo_piyo 72:7d6177047823 179 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)));
echo_piyo 45:a32e8091901b 180 counter ++;
echo_piyo 45:a32e8091901b 181 break;
echo_piyo 45:a32e8091901b 182 case 2:
echo_piyo 74:98bad67fde39 183 rs422.put(id[counter], limitf(0.0*POWER::SHOT,1.0,-1.0), limitf(0.0*POWER::RELOAD,1.0,-1.0));
echo_piyo 45:a32e8091901b 184 counter ++;
echo_piyo 45:a32e8091901b 185 break;
echo_piyo 45:a32e8091901b 186 case 3:
echo_piyo 74:98bad67fde39 187 rs422.put(id[counter], limitf(0.0*POWER::SWORD,1.0,-1.0), limitf(0.0,1.0,-1.0));
echo_piyo 45:a32e8091901b 188 counter ++;
echo_piyo 45:a32e8091901b 189 break;
echo_piyo 45:a32e8091901b 190 default:
echo_piyo 45:a32e8091901b 191 break;
echo_piyo 48:96b5f5ebdfb0 192 }
echo_piyo 45:a32e8091901b 193 }else{
echo_piyo 48:96b5f5ebdfb0 194 switch (counter) {
echo_piyo 48:96b5f5ebdfb0 195 case 0:
echo_piyo 48:96b5f5ebdfb0 196 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 197 counter++;
echo_piyo 48:96b5f5ebdfb0 198 break;
echo_piyo 48:96b5f5ebdfb0 199 case 1:
echo_piyo 48:96b5f5ebdfb0 200 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 201 counter ++;
echo_piyo 48:96b5f5ebdfb0 202 break;
echo_piyo 48:96b5f5ebdfb0 203 case 2:
echo_piyo 48:96b5f5ebdfb0 204 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 205 counter ++;
echo_piyo 48:96b5f5ebdfb0 206 break;
echo_piyo 48:96b5f5ebdfb0 207 case 3:
echo_piyo 48:96b5f5ebdfb0 208 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 209 counter ++;
echo_piyo 48:96b5f5ebdfb0 210 break;
echo_piyo 48:96b5f5ebdfb0 211 default:
echo_piyo 48:96b5f5ebdfb0 212 break;
echo_piyo 45:a32e8091901b 213 }
echo_piyo 45:a32e8091901b 214 }
echo_piyo 14:aac2f18f6779 215 }