NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2021_NHK_A

2021 nhk A team

Dependencies:   mbed QEI　led beep softPWM Servo_softpwm IR2302 lpf

main.cpp@30:00a513858a44, 2021-10-30 (annotated)

Committer:
THtakahiro702286
Date:
Sat Oct 30 07:56:14 2021 +0000
Revision:
30:00a513858a44
Parent:
29:3bd99866801f
Child:
31:ce079259f559
add set mode

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