![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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@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?
User | Revision | Line number | New 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 | } |