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@74:98bad67fde39, 2017-10-22 (annotated)

Committer:
echo_piyo
Date:
Sun Oct 22 10:55:47 2017 +0000
Revision:
74:98bad67fde39
Parent:
72:7d6177047823
Child:
75:cd507886ee57
(??)Define?rs442????????

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 74:98bad67fde39 27 #define SHOULDER_LEFT_HIGH p1
echo_piyo 74:98bad67fde39 28 #define SHOULDER_LEFT_LOW p1
echo_piyo 74:98bad67fde39 29 #define SHOULDER_RIGHT_HIGH p2
echo_piyo 74:98bad67fde39 30 #define SHOULDER_RIGHT_LOW p2
echo_piyo 72:7d6177047823 31
echo_piyo 74:98bad67fde39 32 #define GROUND_ENCORDER_X_A p3
echo_piyo 74:98bad67fde39 33 #define GROUND_ENCORDER_X_B p4
echo_piyo 74:98bad67fde39 34 #define GROUND_ENCORDER_Y_A p5
echo_piyo 74:98bad67fde39 35 #define GROUND_ENCORDER_Y_B p6
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 0:bf96e953cdb8 98
echo_piyo 0:bf96e953cdb8 99 int main()
echo_piyo 0:bf96e953cdb8 100 {
echo_piyo 0:bf96e953cdb8 101 setup();
echo_piyo 2:d5b8f8e62923 102 while(1) {
echo_piyo 72:7d6177047823 103 //pc.printf("cylinder pos : %f\r\n",shotEncoder.deg());
echo_piyo 55:2dd2f161ebaf 104 //pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall());
echo_piyo 0:bf96e953cdb8 105 }
echo_piyo 0:bf96e953cdb8 106 }
echo_piyo 0:bf96e953cdb8 107
echo_piyo 0:bf96e953cdb8 108 void setup()
echo_piyo 0:bf96e953cdb8 109 {
echo_piyo 72:7d6177047823 110 wait(1.0);
echo_piyo 65:5e4c2e5494ae 111 bno055.begin();
echo_piyo 40:2d6888448ab2 112 wait(1.0);
echo_piyo 65:5e4c2e5494ae 113 bno055.firstRead();
echo_piyo 71:7d684ff43ddd 114 pc.baud(BAUD::PC);
echo_piyo 71:7d684ff43ddd 115 sbdbt.begin(BAUD::SBDBT);
echo_piyo 71:7d684ff43ddd 116 rs422.begin(BAUD::RS422);
echo_piyo 72:7d6177047823 117 outputTimer.attach(&output, CYCLE::OUTPUT);
echo_piyo 72:7d6177047823 118 mecanum.setupdeg(bno055.getYawRad()); //基盤が前後逆の場合+180
echo_piyo 71:7d684ff43ddd 119 v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
echo_piyo 71:7d684ff43ddd 120 v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
echo_piyo 71:7d684ff43ddd 121 v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
echo_piyo 71:7d684ff43ddd 122 v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
echo_piyo 10:04f2a82cfd89 123 }
echo_piyo 10:04f2a82cfd89 124
echo_piyo 72:7d6177047823 125 //index_mecanum
echo_piyo 72:7d6177047823 126 void motorCalculation(){
echo_piyo 56:a7bd860b85b6 127 static float out_right_x;
echo_piyo 72:7d6177047823 128 yaw::now = bno055.getYawRad();
echo_piyo 72:7d6177047823 129 yaw::target = yaw::now;
echo_piyo 72:7d6177047823 130 yawPid.calculate(yaw::target, yaw::now);
echo_piyo 56:a7bd860b85b6 131
echo_piyo 56:a7bd860b85b6 132 //後進時に方向が逆転するため
echo_piyo 56:a7bd860b85b6 133 if(sbdbt.right_y < 0.0){
echo_piyo 56:a7bd860b85b6 134 out_right_x = -1.0*sbdbt.right_x;
echo_piyo 56:a7bd860b85b6 135 }else{
echo_piyo 56:a7bd860b85b6 136 out_right_x = sbdbt.right_x;
echo_piyo 56:a7bd860b85b6 137 }
echo_piyo 56:a7bd860b85b6 138 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, bno055.getYawRad());
echo_piyo 5:6efda58ff71b 139 // 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 140 }
echo_piyo 72:7d6177047823 141
echo_piyo 74:98bad67fde39 142
echo_piyo 72:7d6177047823 143 //index_output
echo_piyo 35:5e1ad00f26fb 144 void output()
echo_piyo 35:5e1ad00f26fb 145 {
echo_piyo 72:7d6177047823 146 motorCalculation();
echo_piyo 72:7d6177047823 147 controlIndigator = sbdbt.get_pairingState();
echo_piyo 48:96b5f5ebdfb0 148 //led1 = sbdbt.get_pairingState();
echo_piyo 49:b041c815c063 149 if(sbdbt.left)bno055.yaw_origin();
echo_piyo 72:7d6177047823 150
echo_piyo 35:5e1ad00f26fb 151 static int counter;
echo_piyo 74:98bad67fde39 152 int id[nucleo_num] = {n0_id, n1_id, n2_id, n3_id};
echo_piyo 45:a32e8091901b 153
echo_piyo 45:a32e8091901b 154 //sbdbtがpairingしている場合のみ動作
echo_piyo 45:a32e8091901b 155 if(sbdbt.get_pairingState()) {
echo_piyo 45:a32e8091901b 156 switch (counter) {
echo_piyo 45:a32e8091901b 157 case 0:
echo_piyo 72:7d6177047823 158 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 159 counter++;
echo_piyo 45:a32e8091901b 160 break;
echo_piyo 45:a32e8091901b 161 case 1:
echo_piyo 72:7d6177047823 162 //.duty(<cal>*<powerControle>+(<motorBoost>*0.5))
echo_piyo 72:7d6177047823 163 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 164 counter ++;
echo_piyo 45:a32e8091901b 165 break;
echo_piyo 45:a32e8091901b 166 case 2:
echo_piyo 74:98bad67fde39 167 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 168 counter ++;
echo_piyo 45:a32e8091901b 169 break;
echo_piyo 45:a32e8091901b 170 case 3:
echo_piyo 74:98bad67fde39 171 rs422.put(id[counter], limitf(0.0*POWER::SWORD,1.0,-1.0), limitf(0.0,1.0,-1.0));
echo_piyo 45:a32e8091901b 172 counter ++;
echo_piyo 45:a32e8091901b 173 break;
echo_piyo 45:a32e8091901b 174 default:
echo_piyo 45:a32e8091901b 175 break;
echo_piyo 48:96b5f5ebdfb0 176 }
echo_piyo 45:a32e8091901b 177 }else{
echo_piyo 48:96b5f5ebdfb0 178 switch (counter) {
echo_piyo 48:96b5f5ebdfb0 179 case 0:
echo_piyo 48:96b5f5ebdfb0 180 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 181 counter++;
echo_piyo 48:96b5f5ebdfb0 182 break;
echo_piyo 48:96b5f5ebdfb0 183 case 1:
echo_piyo 48:96b5f5ebdfb0 184 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 185 counter ++;
echo_piyo 48:96b5f5ebdfb0 186 break;
echo_piyo 48:96b5f5ebdfb0 187 case 2:
echo_piyo 48:96b5f5ebdfb0 188 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 189 counter ++;
echo_piyo 48:96b5f5ebdfb0 190 break;
echo_piyo 48:96b5f5ebdfb0 191 case 3:
echo_piyo 48:96b5f5ebdfb0 192 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 193 counter ++;
echo_piyo 48:96b5f5ebdfb0 194 break;
echo_piyo 48:96b5f5ebdfb0 195 default:
echo_piyo 48:96b5f5ebdfb0 196 break;
echo_piyo 45:a32e8091901b 197 }
echo_piyo 45:a32e8091901b 198 }
echo_piyo 14:aac2f18f6779 199 }