NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2021_NHK_A

2021 nhk A team

Dependencies:   mbed QEI　led beep softPWM Servo_softpwm IR2302 lpf

main.cpp@29:3bd99866801f, 2021-10-30 (annotated)

Committer:
THtakahiro702286
Date:
Sat Oct 30 03:14:36 2021 +0000
Revision:
29:3bd99866801f
Parent:
28:a77d8d164630
Child:
30:00a513858a44
10/30 success

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 27:818783bc9a89 1 //2021/10/23 紐掴むだけ
THtakahiro702286 14:7e5b21c812fd 2
THtakahiro702286 0:6f4b2005b633 3 #include "mbed.h"
umekou 12:915a390e3fc4 4
THtakahiro702286 0:6f4b2005b633 5 #include "jy901.h"
Washio 11:b041e6cea0ef 6 #include "Servo.h"
Washio 11:b041e6cea0ef 7 #include "QEI.h"
umekou 13:9ad33de1c942 8 #include "IR2302.h"
umekou 12:915a390e3fc4 9
Washio 11:b041e6cea0ef 10 #include "pin_config.h"
Washio 11:b041e6cea0ef 11 #include "control_parameter.h"
umekou 12:915a390e3fc4 12
THtakahiro702286 0:6f4b2005b633 13 #include "lpf.h"
THtakahiro702286 0:6f4b2005b633 14 #include "beep.h"
THtakahiro702286 15:d8db04a67df9 15
THtakahiro702286 15:d8db04a67df9 16 Serial pc(USBTX,USBRX,115200);
THtakahiro702286 18:20dee7c7a628 17
THtakahiro702286 18:20dee7c7a628 18 //Beep buzzer(BEEP);
THtakahiro702286 0:6f4b2005b633 19 JY901 jy(SDA, SCL);
THtakahiro702286 18:20dee7c7a628 20 //DigitalOut stop(STOP);
THtakahiro702286 18:20dee7c7a628 21
THtakahiro702286 0:6f4b2005b633 22 QEI encoder(QEI1 ,QEI2 ,NC ,100 ,QEI::X4_ENCODING);
THtakahiro702286 18:20dee7c7a628 23
THtakahiro702286 0:6f4b2005b633 24 lpf yAcc(LPF_C_TIME, LPF_A_TIME);
THtakahiro702286 0:6f4b2005b633 25 lpf zAcc(LPF_C_TIME, LPF_A_TIME);
THtakahiro702286 21:b7734064c7dc 26 lpf mSp(MOT_C_TIME, MOT_A_TIME);
umekou 7:74b1ad3a2450 27 Servo servo[3] = {SERVO1, SERVO2, SERVO3};
umekou 13:9ad33de1c942 28 IR2302 ir2302(MD_A,MD_B);//梅沢追加　IR2302
THtakahiro702286 24:952a01c8baee 29 Timer setWait,nextWait,jumpWait;
THtakahiro702286 26:7cb2c322153a 30 AnalogIn ph[3] = {PHOTO1,PHOTO2,PHOTO3};
THtakahiro702286 26:7cb2c322153a 31 DigitalOut light[3] = {ROPE1,ROPE2,ROPE3};
THtakahiro702286 27:818783bc9a89 32 DigitalIn b(USER_BUTTON);
THtakahiro702286 27:818783bc9a89 33
THtakahiro702286 0:6f4b2005b633 34 int main()
THtakahiro702286 0:6f4b2005b633 35 {
THtakahiro702286 29:3bd99866801f 36 int i=0,j=0,k=3,l=0;//0,0,2,0
THtakahiro702286 18:20dee7c7a628 37 double acc[3]; //加速度
THtakahiro702286 18:20dee7c7a628 38 double abac[3]={0};//初期座標軸での加速度
THtakahiro702286 18:20dee7c7a628 39 double angle[3];
THtakahiro702286 0:6f4b2005b633 40 float lpfY,lpfZ; //ローパス後の加速度
THtakahiro702286 21:b7734064c7dc 41 float lpfM; //モータのローパス
THtakahiro702286 0:6f4b2005b633 42 jy.calibrateAll(50);
umekou 12:915a390e3fc4 43
THtakahiro702286 0:6f4b2005b633 44 int nowPalse;
THtakahiro702286 0:6f4b2005b633 45
THtakahiro702286 26:7cb2c322153a 46 float phI[3] = {0};//フォトインタラプタ
THtakahiro702286 0:6f4b2005b633 47 bool rope = 0;
THtakahiro702286 20:c51583296f7e 48
THtakahiro702286 27:818783bc9a89 49 bool firstFlag = 1;//1
THtakahiro702286 24:952a01c8baee 50 bool secondFlag= 0;//0
THtakahiro702286 24:952a01c8baee 51 bool thirdFlag = 0;//0
THtakahiro702286 20:c51583296f7e 52 bool swingFlag = 1;//1
THtakahiro702286 14:7e5b21c812fd 53 bool flyFlag = 0;//0
THtakahiro702286 14:7e5b21c812fd 54 bool catchFlag = 0;//0
THtakahiro702286 14:7e5b21c812fd 55 int8_t way = 1;//1or-1
THtakahiro702286 14:7e5b21c812fd 56 bool resetFlag = 0;//0
THtakahiro702286 0:6f4b2005b633 57 float fiveYAcc[5]={0};
THtakahiro702286 0:6f4b2005b633 58
THtakahiro702286 0:6f4b2005b633 59 float servoAngle[3];
THtakahiro702286 27:818783bc9a89 60 float motor=0;
THtakahiro702286 0:6f4b2005b633 61
THtakahiro702286 21:b7734064c7dc 62 for(int i=0;i < 3; i++)servo[i].calibrate(0.00095, 90);
THtakahiro702286 29:3bd99866801f 63 servoAngle[0] = 0.2;
THtakahiro702286 29:3bd99866801f 64 servoAngle[1] = 0.2;
THtakahiro702286 29:3bd99866801f 65 servoAngle[2] = 0.82;
THtakahiro702286 27:818783bc9a89 66 for(i=0;i<3;i++) servo[i].write(servoAngle[i]);
THtakahiro702286 26:7cb2c322153a 67 for(i=0; i<3; i++) light[i] = true;
THtakahiro702286 26:7cb2c322153a 68
THtakahiro702286 16:bdc10f57c11a 69
THtakahiro702286 21:b7734064c7dc 70 wait(5);
THtakahiro702286 16:bdc10f57c11a 71
THtakahiro702286 0:6f4b2005b633 72 while(1)
THtakahiro702286 0:6f4b2005b633 73 {
THtakahiro702286 0:6f4b2005b633 74 //センシング
THtakahiro702286 24:952a01c8baee 75 /*
THtakahiro702286 0:6f4b2005b633 76 //jy901
THtakahiro702286 0:6f4b2005b633 77 acc[1] = jy.getYaxisAcceleration()*10;
THtakahiro702286 0:6f4b2005b633 78 acc[2] = jy.getZaxisAcceleration()*10;
THtakahiro702286 0:6f4b2005b633 79 angle[0] = jy.getXaxisAngle();
THtakahiro702286 0:6f4b2005b633 80 angle[1] = jy.getYaxisAngle();
THtakahiro702286 0:6f4b2005b633 81 angle[2] = jy.getZaxisAngle();
THtakahiro702286 0:6f4b2005b633 82 for(i=0; i<3; i++)
THtakahiro702286 0:6f4b2005b633 83 {
THtakahiro702286 0:6f4b2005b633 84 if(acc[i] > 1567)acc[i] -= 3134;
THtakahiro702286 0:6f4b2005b633 85 }
THtakahiro702286 15:d8db04a67df9 86 abac[1] = acc[1]*cos(angle[0]/180.0*PI) - acc[2] * sin(angle[0]/180.0*PI);
THtakahiro702286 15:d8db04a67df9 87 abac[2] = acc[2]*cos(angle[0]/180.0*PI) + acc[1] * sin(angle[0]/180.0*PI);
THtakahiro702286 18:20dee7c7a628 88
THtakahiro702286 0:6f4b2005b633 89 //ローパスフィルタ
THtakahiro702286 0:6f4b2005b633 90 lpfY = yAcc.path_value(abac[1]);
THtakahiro702286 0:6f4b2005b633 91 lpfZ = zAcc.path_value(abac[2]);
THtakahiro702286 18:20dee7c7a628 92
THtakahiro702286 0:6f4b2005b633 93 //swingで使うやつ
THtakahiro702286 0:6f4b2005b633 94 fiveYAcc[j%5]= lpfY;
THtakahiro702286 0:6f4b2005b633 95 j++;
THtakahiro702286 24:952a01c8baee 96 */
THtakahiro702286 28:a77d8d164630 97 //ロータリーエンコーダ 向き変ったので-1掛け
THtakahiro702286 28:a77d8d164630 98 nowPalse = encoder.getPulses() * -1;
THtakahiro702286 0:6f4b2005b633 99
THtakahiro702286 0:6f4b2005b633 100 //紐センサ
THtakahiro702286 26:7cb2c322153a 101 for(i=0; i<3; i++) phI[i]=ph[i];
THtakahiro702286 0:6f4b2005b633 102
THtakahiro702286 0:6f4b2005b633 103 //処理部
THtakahiro702286 18:20dee7c7a628 104
THtakahiro702286 24:952a01c8baee 105 //加速1段目
THtakahiro702286 24:952a01c8baee 106 if(firstFlag)
THtakahiro702286 0:6f4b2005b633 107 {
THtakahiro702286 24:952a01c8baee 108 motor = FIRST_POWER * way * swingFlag; //振り子を振る、wayで反転を制御
THtakahiro702286 21:b7734064c7dc 109 motor = mSp.path_value(motor);
THtakahiro702286 20:c51583296f7e 110 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
THtakahiro702286 0:6f4b2005b633 111 {
THtakahiro702286 0:6f4b2005b633 112 way *= -1;
THtakahiro702286 24:952a01c8baee 113 nextWait.start();
THtakahiro702286 0:6f4b2005b633 114 setWait.start();
THtakahiro702286 0:6f4b2005b633 115 swingFlag = 0;
THtakahiro702286 25:8bbfa2fa59be 116 if(nextWait > SECOND_LINE) //一定時間経過したらsecondに移行
THtakahiro702286 21:b7734064c7dc 117 {
THtakahiro702286 24:952a01c8baee 118 firstFlag = 0;
THtakahiro702286 24:952a01c8baee 119 secondFlag = 1;
THtakahiro702286 21:b7734064c7dc 120 swingFlag = 0;
THtakahiro702286 24:952a01c8baee 121 nextWait.stop();
THtakahiro702286 24:952a01c8baee 122 nextWait.reset();
THtakahiro702286 21:b7734064c7dc 123 }
THtakahiro702286 0:6f4b2005b633 124 }
THtakahiro702286 24:952a01c8baee 125 if(setWait > FIRST_WAIT)
THtakahiro702286 0:6f4b2005b633 126 {
THtakahiro702286 0:6f4b2005b633 127 setWait.stop();
THtakahiro702286 0:6f4b2005b633 128 setWait.reset();
THtakahiro702286 0:6f4b2005b633 129 swingFlag = 1;
THtakahiro702286 0:6f4b2005b633 130 }
THtakahiro702286 0:6f4b2005b633 131 }
THtakahiro702286 24:952a01c8baee 132
THtakahiro702286 24:952a01c8baee 133 //加速2段目
THtakahiro702286 24:952a01c8baee 134 if(secondFlag)
THtakahiro702286 0:6f4b2005b633 135 {
THtakahiro702286 24:952a01c8baee 136 motor = SECOND_POWER * way * swingFlag; //振り子を振る、wayで反転を制御
THtakahiro702286 24:952a01c8baee 137 nextWait.start();
THtakahiro702286 24:952a01c8baee 138 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
THtakahiro702286 0:6f4b2005b633 139 {
THtakahiro702286 0:6f4b2005b633 140 way *= -1;
THtakahiro702286 24:952a01c8baee 141 setWait.start();
THtakahiro702286 0:6f4b2005b633 142 swingFlag = 0;
THtakahiro702286 25:8bbfa2fa59be 143 if(nextWait > THIRD_LINE) //一定時間経過したらthirdに移行
THtakahiro702286 24:952a01c8baee 144 {
THtakahiro702286 24:952a01c8baee 145 secondFlag = 0;
THtakahiro702286 24:952a01c8baee 146 thirdFlag = 1;
THtakahiro702286 24:952a01c8baee 147 swingFlag = 0;
THtakahiro702286 24:952a01c8baee 148 nextWait.stop();
THtakahiro702286 24:952a01c8baee 149 nextWait.reset();
THtakahiro702286 24:952a01c8baee 150 }
THtakahiro702286 0:6f4b2005b633 151 }
THtakahiro702286 24:952a01c8baee 152 if(setWait > SECOND_WAIT)
THtakahiro702286 24:952a01c8baee 153 {
THtakahiro702286 24:952a01c8baee 154 setWait.stop();
THtakahiro702286 24:952a01c8baee 155 setWait.reset();
THtakahiro702286 24:952a01c8baee 156 swingFlag = 1;
THtakahiro702286 24:952a01c8baee 157 }
THtakahiro702286 0:6f4b2005b633 158 }
THtakahiro702286 21:b7734064c7dc 159
THtakahiro702286 24:952a01c8baee 160 //加速3段目
THtakahiro702286 24:952a01c8baee 161 if(thirdFlag)
THtakahiro702286 0:6f4b2005b633 162 {
THtakahiro702286 24:952a01c8baee 163 jumpWait.start();
THtakahiro702286 24:952a01c8baee 164 motor = THIRD_POWER * way * swingFlag; //振り子を振る、wayで反転を制御
THtakahiro702286 24:952a01c8baee 165 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
THtakahiro702286 24:952a01c8baee 166 {
THtakahiro702286 24:952a01c8baee 167 way *= -1;
THtakahiro702286 24:952a01c8baee 168 nextWait.start();
THtakahiro702286 24:952a01c8baee 169 setWait.start();
THtakahiro702286 24:952a01c8baee 170 swingFlag = 0;
THtakahiro702286 24:952a01c8baee 171 }
THtakahiro702286 24:952a01c8baee 172 if(setWait > THIRD_WAIT)
THtakahiro702286 24:952a01c8baee 173 {
THtakahiro702286 24:952a01c8baee 174 setWait.stop();
THtakahiro702286 24:952a01c8baee 175 setWait.reset();
THtakahiro702286 24:952a01c8baee 176 swingFlag = 1;
THtakahiro702286 24:952a01c8baee 177 }
THtakahiro702286 27:818783bc9a89 178 if((jumpWait > JUMP_TIME) && (nowPalse < JUMP_LINE) && (nowPalse > JUMP_LINE-10) && (way == -1)) //一定時間たったら前に飛ぶ
THtakahiro702286 24:952a01c8baee 179 {
THtakahiro702286 26:7cb2c322153a 180 flyFlag = 1;
THtakahiro702286 26:7cb2c322153a 181 thirdFlag = 0;
THtakahiro702286 24:952a01c8baee 182 }
THtakahiro702286 0:6f4b2005b633 183 }
THtakahiro702286 27:818783bc9a89 184
THtakahiro702286 27:818783bc9a89 185
THtakahiro702286 0:6f4b2005b633 186 //ジャンプ
THtakahiro702286 0:6f4b2005b633 187 if(flyFlag)
THtakahiro702286 0:6f4b2005b633 188 {
THtakahiro702286 27:818783bc9a89 189 // if(lpfZ < RELEASE_LINE)
THtakahiro702286 0:6f4b2005b633 190 {
THtakahiro702286 28:a77d8d164630 191 servoAngle[(k-1)%3] = 0.1;//サーボ離す
THtakahiro702286 29:3bd99866801f 192 servoAngle[k%3] = 0.7;
THtakahiro702286 28:a77d8d164630 193 // l++;
THtakahiro702286 28:a77d8d164630 194 l=1;
THtakahiro702286 0:6f4b2005b633 195 flyFlag = 0;
THtakahiro702286 27:818783bc9a89 196 catchFlag=1;
THtakahiro702286 27:818783bc9a89 197 }
THtakahiro702286 27:818783bc9a89 198 motor=0;
THtakahiro702286 27:818783bc9a89 199
THtakahiro702286 0:6f4b2005b633 200 }
THtakahiro702286 27:818783bc9a89 201
THtakahiro702286 28:a77d8d164630 202
THtakahiro702286 0:6f4b2005b633 203 //キャッチ
THtakahiro702286 0:6f4b2005b633 204 if(catchFlag)
THtakahiro702286 0:6f4b2005b633 205 {
THtakahiro702286 27:818783bc9a89 206 if(phI[k%3] < ROPE_ACRROS[k%3])
THtakahiro702286 27:818783bc9a89 207 {
THtakahiro702286 27:818783bc9a89 208 catchFlag=0;
THtakahiro702286 27:818783bc9a89 209 motor = 0;
THtakahiro702286 28:a77d8d164630 210 resetFlag=1;
THtakahiro702286 28:a77d8d164630 211 k++;
THtakahiro702286 27:818783bc9a89 212 }
THtakahiro702286 27:818783bc9a89 213
THtakahiro702286 0:6f4b2005b633 214 }
THtakahiro702286 28:a77d8d164630 215
THtakahiro702286 0:6f4b2005b633 216 //掴んだ後
THtakahiro702286 0:6f4b2005b633 217 if(resetFlag)
THtakahiro702286 0:6f4b2005b633 218 {
THtakahiro702286 28:a77d8d164630 219 if(nowPalse < NEXT_PALSE * l)
THtakahiro702286 28:a77d8d164630 220 {
THtakahiro702286 28:a77d8d164630 221 motor=0.35;
THtakahiro702286 28:a77d8d164630 222 }
THtakahiro702286 28:a77d8d164630 223 else
THtakahiro702286 28:a77d8d164630 224 {
THtakahiro702286 28:a77d8d164630 225 motor=0;
THtakahiro702286 28:a77d8d164630 226 resetFlag = 0;
THtakahiro702286 28:a77d8d164630 227 // firstFlag = 1;
THtakahiro702286 28:a77d8d164630 228 }
THtakahiro702286 0:6f4b2005b633 229 }
THtakahiro702286 28:a77d8d164630 230
THtakahiro702286 14:7e5b21c812fd 231 //実験用記述-----------
THtakahiro702286 27:818783bc9a89 232 /*
THtakahiro702286 22:efa233983e1f 233 if(flyFlag)
THtakahiro702286 22:efa233983e1f 234 {
THtakahiro702286 22:efa233983e1f 235 // if(lpfZ < RELEASELINE)
THtakahiro702286 22:efa233983e1f 236 {
THtakahiro702286 22:efa233983e1f 237 for(i=0; i < 3; i++) servoAngle[i] = -1.0;//サーボ離す
THtakahiro702286 22:efa233983e1f 238 // k++;
THtakahiro702286 22:efa233983e1f 239 flyFlag = 0;
THtakahiro702286 22:efa233983e1f 240 }
THtakahiro702286 22:efa233983e1f 241 motor=0;
THtakahiro702286 22:efa233983e1f 242 }
THtakahiro702286 27:818783bc9a89 243 */
THtakahiro702286 27:818783bc9a89 244 /*
THtakahiro702286 27:818783bc9a89 245 for(i=0; i<3; i++)
THtakahiro702286 27:818783bc9a89 246 {
THtakahiro702286 27:818783bc9a89 247 if(phI[i] < ROPE_ACRROS[i]) servoAngle[i] = -0.8;
THtakahiro702286 27:818783bc9a89 248
THtakahiro702286 27:818783bc9a89 249 }
THtakahiro702286 27:818783bc9a89 250 */
THtakahiro702286 27:818783bc9a89 251 /*
THtakahiro702286 27:818783bc9a89 252 if(phI[k%3] < ROPE_ACRROS[k%3])
THtakahiro702286 27:818783bc9a89 253 {
THtakahiro702286 27:818783bc9a89 254 servoAngle[k%3] = 0.85;
THtakahiro702286 28:a77d8d164630 255 servoAngle[(k+1)%3] = 0.4;
THtakahiro702286 28:a77d8d164630 256 k++;
THtakahiro702286 27:818783bc9a89 257 }
THtakahiro702286 27:818783bc9a89 258 */
THtakahiro702286 27:818783bc9a89 259 // if(!b) motor=0.3;
THtakahiro702286 27:818783bc9a89 260 // else motor=0;
THtakahiro702286 22:efa233983e1f 261
THtakahiro702286 14:7e5b21c812fd 262 //-------------------
umekou 12:915a390e3fc4 263
THtakahiro702286 0:6f4b2005b633 264 //駆動系
umekou 12:915a390e3fc4 265
THtakahiro702286 0:6f4b2005b633 266 //サーボ
THtakahiro702286 0:6f4b2005b633 267 for(i=0;i<3;i++) servo[i].write(servoAngle[i]);
THtakahiro702286 0:6f4b2005b633 268
THtakahiro702286 0:6f4b2005b633 269 //モーター
THtakahiro702286 20:c51583296f7e 270 ir2302.SetMotorSpeed(motor);
THtakahiro702286 18:20dee7c7a628 271
THtakahiro702286 15:d8db04a67df9 272 //表示系
THtakahiro702286 18:20dee7c7a628 273 // pc.printf("%.2f,%.2f,%.2f",angle[0],angle[1],angle[2]);
THtakahiro702286 24:952a01c8baee 274 // pc.printf("%.2f,%.2f,%.2f,",servoAngle[0],servoAngle[1],servoAngle[2]);
THtakahiro702286 24:952a01c8baee 275 // pc.printf("%.4f,",motor);
THtakahiro702286 18:20dee7c7a628 276 // pc.printf("%.2f,%.2f,",abac[1],abac[2]);
THtakahiro702286 22:efa233983e1f 277 // pc.printf("%.2f,%.2f,",lpfY,lpfZ);
THtakahiro702286 19:b9f53bc0fdf2 278 // pc.printf("%d,",nowPalse);
THtakahiro702286 27:818783bc9a89 279 // pc.printf("%d,",catchFlag);
THtakahiro702286 28:a77d8d164630 280 // pc.printf("%d,",resetFlag);
THtakahiro702286 21:b7734064c7dc 281 // pc.printf("%.2f,",lpfM);
THtakahiro702286 27:818783bc9a89 282 // pc.printf("%.3f,%.3f,%.3f,",phI[0],phI[1],phI[2]);
THtakahiro702286 28:a77d8d164630 283 // pc.printf("%d,",k);
THtakahiro702286 27:818783bc9a89 284 // pc.printf("\r\n");
THtakahiro702286 18:20dee7c7a628 285
THtakahiro702286 0:6f4b2005b633 286 }
THtakahiro702286 0:6f4b2005b633 287 }
umekou 12:915a390e3fc4 288
umekou 12:915a390e3fc4 289