MEC-B / Mbed 2 deprecated AR_MastarNode_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   DriveConroller IMU MDD Mycan Odometer PID RotaryEncoder UART USS mbed

Fork of AR_MastarNode by MEC-B

main.cpp@6:a102603c99fd, 2018-08-17 (annotated)

Committer:
TanakaTarou
Date:
Fri Aug 17 08:00:09 2018 +0000
Revision:
6:a102603c99fd
Parent:
5:7c5e07260e1e
Child:
7:1ee46b2e8dce
8/17 16:59

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaTarou 0:6db16ad02a1b 1 #include "mbed.h"
TanakaTarou 0:6db16ad02a1b 2 #include "DriveController.h"
TanakaTarou 0:6db16ad02a1b 3 #include "IMU.h"
TanakaTarou 0:6db16ad02a1b 4 #include "MDD.h"
TanakaTarou 0:6db16ad02a1b 5 #include "MDD2.h"
TanakaTarou 0:6db16ad02a1b 6 #include "Mycan.h"
TanakaTarou 0:6db16ad02a1b 7 #include "Odometer.h"
TanakaTarou 0:6db16ad02a1b 8 #include "PID.h"
TanakaTarou 0:6db16ad02a1b 9 #include "RotaryEncoder.h"
TanakaTarou 0:6db16ad02a1b 10 #include "CSV.h"
TanakaTarou 0:6db16ad02a1b 11 #include "SBUS.h"
TanakaTarou 0:6db16ad02a1b 12 #include "USS.h"
TanakaTarou 0:6db16ad02a1b 13 #include "hardwareConfig.h"
TanakaTarou 0:6db16ad02a1b 14
TanakaTarou 4:9f74525eb37f 15
TanakaTarou 0:6db16ad02a1b 16 //(X, Y, θ, speed, angle, injection, 補給昇降)
TanakaTarou 4:9f74525eb37f 17 float position[19][6] = {{0.0, 0.0, 0.0, 0, 90, 0},//初期位置
TanakaTarou 6:a102603c99fd 18 {-1.825, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化
TanakaTarou 6:a102603c99fd 19 {-1.825, 18.0, 0.0, 0, 82, 0},//y移動
TanakaTarou 6:a102603c99fd 20 {-1.825, 18.0, 0.0, 35, 82, 0},//発射
TanakaTarou 6:a102603c99fd 21 {-1.825, 0.0, 0.0, 0, 88, 1},//y移動,角度戻す, P上昇
TanakaTarou 6:a102603c99fd 22
TanakaTarou 6:a102603c99fd 23 {-2.825, 0.0, 0.0, 0, 88, 1},//x移動, 角度変化, 下降&バットマン駆動
TanakaTarou 6:a102603c99fd 24 {-2.825, 18.0, 0.0, 0, 82, 0},//y移動
TanakaTarou 6:a102603c99fd 25 {-2.825, 18.0, 0.0, 35, 82, 0},//発射
TanakaTarou 6:a102603c99fd 26 {-2.825, 0.0, 0.0, 0, 88, 1},//y移動,角度戻す, P上昇
TanakaTarou 6:a102603c99fd 27
TanakaTarou 6:a102603c99fd 28 {-3.825, 0.0, 0.0, 0, 88, 1},//x移動, 角度変化, 下降&バットマン駆動
TanakaTarou 5:7c5e07260e1e 29 {-3.825, 18.0, 0.0, 0, 82, 0},//y移動
TanakaTarou 5:7c5e07260e1e 30 {-3.825, 18.0, 0.0, 35, 82, 0},//発射
TanakaTarou 5:7c5e07260e1e 31 {-3.825, 0.0, 0.0, 0, 88, 1},//y移動,角度戻す, P上昇
TanakaTarou 0:6db16ad02a1b 32
TanakaTarou 6:a102603c99fd 33 {-0.825, 0.0, 0.0, 0, 88, 1},//x移動, 角度変化, 下降&バットマン駆動
TanakaTarou 6:a102603c99fd 34 {-0.825, 18.0, 0.0, 0, 82, 0},//y移動
TanakaTarou 6:a102603c99fd 35 {-0.825, 18.0, 0.0, 18, 82, 0},//下段に発射
TanakaTarou 6:a102603c99fd 36 {-0.825, 18.0, 0.0, 0, 88, 1},//角度戻す, P上昇
TanakaTarou 6:a102603c99fd 37 {-0.825, 18.0, 0.0, 0, 85, 0},//角度変化, 下降&バットマン駆動
TanakaTarou 6:a102603c99fd 38 {-0.825, 18.0, 0.0, 18, 85, 0},//上段に発射
TanakaTarou 0:6db16ad02a1b 39 };
TanakaTarou 0:6db16ad02a1b 40
TanakaTarou 0:6db16ad02a1b 41 controller getPropoData();
TanakaTarou 3:6b4adb4d7101 42 bool isConvergetnceTops();
TanakaTarou 0:6db16ad02a1b 43
TanakaTarou 0:6db16ad02a1b 44 //x軸補正用 PID
soyooo 1:d7ceb38da3d8 45 PID pidRobotX(2, 0, 0, 0.01, 0.3, &timer);
TanakaTarou 0:6db16ad02a1b 46 float target_x = 0;
TanakaTarou 0:6db16ad02a1b 47
TanakaTarou 0:6db16ad02a1b 48 //y軸補正用 PID
soyooo 1:d7ceb38da3d8 49 PID pidRobotY(2, 0, 0, 0.01, 0.3, &timer);
TanakaTarou 0:6db16ad02a1b 50 float target_y = 0;
TanakaTarou 0:6db16ad02a1b 51
soyooo 1:d7ceb38da3d8 52 //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
soyooo 1:d7ceb38da3d8 53 PID pidRobotYow(0.05, 0, 0, 0.01, 0.95, &timer);
soyooo 1:d7ceb38da3d8 54 float target_yow = 0;
soyooo 1:d7ceb38da3d8 55
TanakaTarou 0:6db16ad02a1b 56 //USS用
TanakaTarou 5:7c5e07260e1e 57 PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer);
TanakaTarou 4:9f74525eb37f 58 float target_uss = 8.0;
TanakaTarou 3:6b4adb4d7101 59 int posi_num = 0;
TanakaTarou 3:6b4adb4d7101 60
TanakaTarou 0:6db16ad02a1b 61 int main()
TanakaTarou 0:6db16ad02a1b 62 {
TanakaTarou 0:6db16ad02a1b 63 //タイマー３の優先度を最低にする
TanakaTarou 0:6db16ad02a1b 64 NVIC_SetPriority(TIMER3_IRQn, 100);
TanakaTarou 0:6db16ad02a1b 65
TanakaTarou 0:6db16ad02a1b 66 //IMUのキャリブレーション
TanakaTarou 4:9f74525eb37f 67 wait(1);
TanakaTarou 0:6db16ad02a1b 68 imu.performCalibration();
TanakaTarou 0:6db16ad02a1b 69 imu.startAngleComputing();
TanakaTarou 5:7c5e07260e1e 70
TanakaTarou 5:7c5e07260e1e 71 uss.startTriger();
TanakaTarou 0:6db16ad02a1b 72
TanakaTarou 0:6db16ad02a1b 73 for(int i; i < 3; i++)
TanakaTarou 0:6db16ad02a1b 74 enc[i].changeDirection();
TanakaTarou 0:6db16ad02a1b 75
TanakaTarou 0:6db16ad02a1b 76 //オドメーターの定義
TanakaTarou 0:6db16ad02a1b 77 float matrix[3][3] = {{1, 0, 0},
TanakaTarou 0:6db16ad02a1b 78 {0, 1, 0},
TanakaTarou 0:6db16ad02a1b 79 {0, 0, 0}
TanakaTarou 0:6db16ad02a1b 80 };
TanakaTarou 0:6db16ad02a1b 81 Odometer odm(matrix, 0.048);
TanakaTarou 0:6db16ad02a1b 82 float tmp = 0;
TanakaTarou 0:6db16ad02a1b 83 float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp};
TanakaTarou 0:6db16ad02a1b 84 odm.setupOdometerSensors(encoders, &imu.angle[2]);
TanakaTarou 0:6db16ad02a1b 85 odm.startComputingOdometry(0.005, 0, 0, 0);
TanakaTarou 0:6db16ad02a1b 86
TanakaTarou 0:6db16ad02a1b 87 //オドメーターX
TanakaTarou 0:6db16ad02a1b 88 pidRobotX.sensor = &odm.x;
TanakaTarou 0:6db16ad02a1b 89 pidRobotX.target = &target_x;
TanakaTarou 0:6db16ad02a1b 90 pidRobotX.start();
TanakaTarou 0:6db16ad02a1b 91
TanakaTarou 0:6db16ad02a1b 92 //オドメーターY
TanakaTarou 0:6db16ad02a1b 93 pidRobotY.sensor = &odm.y;
TanakaTarou 0:6db16ad02a1b 94 pidRobotY.target = &target_y;
TanakaTarou 0:6db16ad02a1b 95 pidRobotY.start();
soyooo 1:d7ceb38da3d8 96
TanakaTarou 5:7c5e07260e1e 97 pidRobotYow.sensor = &imu.angle[2];
TanakaTarou 5:7c5e07260e1e 98 pidRobotYow.target = &target_yow;
TanakaTarou 5:7c5e07260e1e 99 pidRobotYow.start();
TanakaTarou 5:7c5e07260e1e 100
TanakaTarou 5:7c5e07260e1e 101 pidUss.sensor = &uss.distance;
TanakaTarou 5:7c5e07260e1e 102 pidUss.target = &target_uss;
TanakaTarou 5:7c5e07260e1e 103 pidUss.start();
TanakaTarou 5:7c5e07260e1e 104
TanakaTarou 4:9f74525eb37f 105 //許容誤差
soyooo 1:d7ceb38da3d8 106 pidRobotX.allowable_error = 0.1;
soyooo 1:d7ceb38da3d8 107 pidRobotY.allowable_error = 0.1;
soyooo 1:d7ceb38da3d8 108 pidRobotYow.allowable_error = 2;
soyooo 1:d7ceb38da3d8 109 pidUss.allowable_error = 3;
soyooo 1:d7ceb38da3d8 110
TanakaTarou 4:9f74525eb37f 111 //マイクロスイッチ
TanakaTarou 3:6b4adb4d7101 112 sw1.mode(PullUp);
TanakaTarou 3:6b4adb4d7101 113 sw2.mode(PullUp);
TanakaTarou 3:6b4adb4d7101 114
TanakaTarou 6:a102603c99fd 115 while(1)
TanakaTarou 0:6db16ad02a1b 116 {
TanakaTarou 4:9f74525eb37f 117 controller cmd = getPropoData();
TanakaTarou 0:6db16ad02a1b 118
TanakaTarou 3:6b4adb4d7101 119 can.set(1, 1, int(position[posi_num][3]));
TanakaTarou 3:6b4adb4d7101 120 can.set(1, 2, int(position[posi_num][4]));
TanakaTarou 5:7c5e07260e1e 121 can.set(1, 3, int(position[posi_num][5]));
TanakaTarou 3:6b4adb4d7101 122 while(can.send() == 0);
TanakaTarou 3:6b4adb4d7101 123
TanakaTarou 2:7af15d4ee55a 124 can.read();
TanakaTarou 6:a102603c99fd 125 //int supply_pid = can.get(2, 1);
soyooo 1:d7ceb38da3d8 126
TanakaTarou 3:6b4adb4d7101 127 if(pidRobotX.isConvergence(1) == 1
TanakaTarou 3:6b4adb4d7101 128 && pidRobotYow.isConvergence(1) == 1)
soyooo 1:d7ceb38da3d8 129 {
TanakaTarou 6:a102603c99fd 130 //超音波で近づくとき
TanakaTarou 3:6b4adb4d7101 131 if(position[posi_num][1] >= 10)
TanakaTarou 3:6b4adb4d7101 132 {
TanakaTarou 3:6b4adb4d7101 133 if(pidUss.isConvergence(1) == 1
TanakaTarou 3:6b4adb4d7101 134 && isConvergetnceTops() == 1)
TanakaTarou 3:6b4adb4d7101 135 posi_num++;
TanakaTarou 3:6b4adb4d7101 136 }
TanakaTarou 6:a102603c99fd 137 //超音波使わないとき
TanakaTarou 6:a102603c99fd 138 else if(pidRobotY.isConvergence(1) == 1)
soyooo 1:d7ceb38da3d8 139 posi_num++;
soyooo 1:d7ceb38da3d8 140 }
soyooo 1:d7ceb38da3d8 141
TanakaTarou 4:9f74525eb37f 142 if(posi_num >= 19)
soyooo 1:d7ceb38da3d8 143 posi_num = 0;
soyooo 1:d7ceb38da3d8 144
TanakaTarou 0:6db16ad02a1b 145 //ロボットの移動速度(LX, LY, RX)
TanakaTarou 0:6db16ad02a1b 146 float robot_velocity[3];
TanakaTarou 0:6db16ad02a1b 147
TanakaTarou 0:6db16ad02a1b 148 //yow角調整処理
TanakaTarou 0:6db16ad02a1b 149 *pidRobotYow.target = position[posi_num][2];
TanakaTarou 0:6db16ad02a1b 150 robot_velocity[2] = pidRobotYow.output;
TanakaTarou 0:6db16ad02a1b 151
TanakaTarou 0:6db16ad02a1b 152 //x軸調整処理
TanakaTarou 0:6db16ad02a1b 153 *pidRobotX.target = position[posi_num][0];
TanakaTarou 0:6db16ad02a1b 154 robot_velocity[0] = pidRobotX.output;
TanakaTarou 0:6db16ad02a1b 155
TanakaTarou 0:6db16ad02a1b 156 //USS距離調整処理
soyooo 1:d7ceb38da3d8 157 if(position[posi_num][1] >= 10)
TanakaTarou 0:6db16ad02a1b 158 {
TanakaTarou 5:7c5e07260e1e 159 *pidUss.target = position[posi_num][1] - 10;
TanakaTarou 0:6db16ad02a1b 160 robot_velocity[1] = -pidUss.output;
TanakaTarou 0:6db16ad02a1b 161 }
TanakaTarou 0:6db16ad02a1b 162 else
TanakaTarou 0:6db16ad02a1b 163 {
TanakaTarou 0:6db16ad02a1b 164 //y軸調整処理
TanakaTarou 0:6db16ad02a1b 165 *pidRobotY.target = position[posi_num][1];
TanakaTarou 0:6db16ad02a1b 166 robot_velocity[1] = pidRobotY.output;
TanakaTarou 0:6db16ad02a1b 167 }
TanakaTarou 5:7c5e07260e1e 168 /*
TanakaTarou 3:6b4adb4d7101 169 if(sw1 == 1 && sw2 == 1)
TanakaTarou 4:9f74525eb37f 170 imu.angle[2] = odm.y = 0;
TanakaTarou 5:7c5e07260e1e 171 */
TanakaTarou 0:6db16ad02a1b 172 //ホイール速度計算
TanakaTarou 4:9f74525eb37f 173 mecanum.setVelG(robot_velocity);
TanakaTarou 0:6db16ad02a1b 174 mecanum.computeWheelVel();
TanakaTarou 0:6db16ad02a1b 175 mecanum.rescaleWheelVel();
TanakaTarou 0:6db16ad02a1b 176
TanakaTarou 0:6db16ad02a1b 177 //モーターの駆動
TanakaTarou 0:6db16ad02a1b 178 for(int i = 0; i < 4; i++)
TanakaTarou 3:6b4adb4d7101 179 Motor[i].drive(mecanum.wheel_vel[i]);
TanakaTarou 0:6db16ad02a1b 180
TanakaTarou 3:6b4adb4d7101 181 pc.printf("%.2f\t", odm.x);
TanakaTarou 3:6b4adb4d7101 182 pc.printf("%.2f\t", odm.y);
TanakaTarou 3:6b4adb4d7101 183 pc.printf("%.2f\t", imu.angle[2]);
TanakaTarou 3:6b4adb4d7101 184 pc.printf("%.2f\t", uss.distance);
TanakaTarou 5:7c5e07260e1e 185 pc.printf("%.2f\t", *odm.rotations[0]);
TanakaTarou 5:7c5e07260e1e 186 pc.printf("%.2f\t", *odm.rotations[1]);
TanakaTarou 0:6db16ad02a1b 187 pc.printf("\n");
TanakaTarou 2:7af15d4ee55a 188
TanakaTarou 5:7c5e07260e1e 189 wait(0.02);
TanakaTarou 0:6db16ad02a1b 190 }
TanakaTarou 0:6db16ad02a1b 191 }
TanakaTarou 0:6db16ad02a1b 192
TanakaTarou 0:6db16ad02a1b 193 controller getPropoData()
TanakaTarou 0:6db16ad02a1b 194 {
TanakaTarou 0:6db16ad02a1b 195 float dead_zone = 0.05;
TanakaTarou 0:6db16ad02a1b 196 controller propo;
TanakaTarou 0:6db16ad02a1b 197 sbus.isFailSafe();
TanakaTarou 5:7c5e07260e1e 198
TanakaTarou 0:6db16ad02a1b 199 if(sbus.isFailSafe())
TanakaTarou 0:6db16ad02a1b 200 {
TanakaTarou 0:6db16ad02a1b 201 propo.LX = propo.LY = propo.RX = propo.RY = 0;
TanakaTarou 0:6db16ad02a1b 202 propo.H = propo.A = propo.D = propo.F = propo.G = propo.fail_safe = 0;
TanakaTarou 0:6db16ad02a1b 203 }
TanakaTarou 0:6db16ad02a1b 204 else
TanakaTarou 0:6db16ad02a1b 205 {
TanakaTarou 0:6db16ad02a1b 206 propo.LX = sbus.getStickVal(0) / 255.0;
TanakaTarou 0:6db16ad02a1b 207 propo.LY = sbus.getStickVal(1) / 255.0;
TanakaTarou 0:6db16ad02a1b 208 propo.RX = -sbus.getStickVal(2) / 255.0;
TanakaTarou 0:6db16ad02a1b 209 propo.RY = sbus.getStickVal(3) / 255.0;
TanakaTarou 0:6db16ad02a1b 210 propo.H = sbus.getSwitchVal(0);
TanakaTarou 0:6db16ad02a1b 211 propo.C = sbus.getSwitchVal(1);
TanakaTarou 0:6db16ad02a1b 212 propo.E = sbus.getSwitchVal(2);
TanakaTarou 0:6db16ad02a1b 213 propo.F = sbus.getSwitchVal(3);
TanakaTarou 0:6db16ad02a1b 214 propo.G = sbus.getSwitchVal(4);
TanakaTarou 0:6db16ad02a1b 215 propo.fail_safe = 1;
TanakaTarou 0:6db16ad02a1b 216 }
TanakaTarou 0:6db16ad02a1b 217 if(propo.RX < dead_zone && propo.RX > -dead_zone) propo.RX = 0;
TanakaTarou 0:6db16ad02a1b 218 if(propo.RY < dead_zone && propo.RY > -dead_zone) propo.RY = 0;
TanakaTarou 0:6db16ad02a1b 219 if(propo.LX < dead_zone && propo.LX > -dead_zone) propo.LX = 0;
TanakaTarou 0:6db16ad02a1b 220 if(propo.LY < dead_zone && propo.LY > -dead_zone) propo.LY = 0;
TanakaTarou 0:6db16ad02a1b 221 return propo;
TanakaTarou 0:6db16ad02a1b 222 }
TanakaTarou 0:6db16ad02a1b 223
TanakaTarou 3:6b4adb4d7101 224 bool isConvergetnceTops()
TanakaTarou 0:6db16ad02a1b 225 {
TanakaTarou 4:9f74525eb37f 226 int velocity_pid = can.get(3, 1);
TanakaTarou 4:9f74525eb37f 227 int angle_pid = can.get(3, 2);
TanakaTarou 4:9f74525eb37f 228 int velocity_val = can.get(3, 3);
TanakaTarou 4:9f74525eb37f 229 int angle_val = can.get(3, 4);
TanakaTarou 3:6b4adb4d7101 230
TanakaTarou 4:9f74525eb37f 231 if(angle_pid == 1 && velocity_pid == 1 && velocity_val == position[posi_num][3] && angle_val == position[posi_num][4])
TanakaTarou 4:9f74525eb37f 232 return 1;
TanakaTarou 3:6b4adb4d7101 233 else return 0;
TanakaTarou 4:9f74525eb37f 234 }