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@76:03475cafb326, 2017-10-22 (annotated)

Committer:
echo_piyo
Date:
Sun Oct 22 11:26:25 2017 +0000
Revision:
76:03475cafb326
Parent:
75:cd507886ee57 
(??)????shoulderCalclation???controllerEvent???output?????; ????????????

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