NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2021_NHK_A

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:   mbed QEI　led beep softPWM Servo_softpwm IR2302 lpf

main.cpp@27:818783bc9a89, 2021-10-26 (annotated)

Committer:
THtakahiro702286
Date:
Tue Oct 26 06:39:41 2021 +0000
Revision:
27:818783bc9a89
Parent:
26:7cb2c322153a
Child:
28:a77d8d164630
first 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 18:20dee7c7a628 32
THtakahiro702286 27:818783bc9a89 33 DigitalIn b(USER_BUTTON);
THtakahiro702286 27:818783bc9a89 34
THtakahiro702286 0:6f4b2005b633 35 int main()
THtakahiro702286 0:6f4b2005b633 36 {
THtakahiro702286 27:818783bc9a89 37 int i=0,j=0,k=1,l=0;//0,0,1,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 27:818783bc9a89 50 bool firstFlag = 1;//1
THtakahiro702286 24:952a01c8baee 51 bool secondFlag= 0;//0
THtakahiro702286 24:952a01c8baee 52 bool thirdFlag = 0;//0
THtakahiro702286 20:c51583296f7e 53 bool swingFlag = 1;//1
THtakahiro702286 14:7e5b21c812fd 54 bool flyFlag = 0;//0
THtakahiro702286 14:7e5b21c812fd 55 bool catchFlag = 0;//0
THtakahiro702286 14:7e5b21c812fd 56 int8_t way = 1;//1or-1
THtakahiro702286 14:7e5b21c812fd 57 bool resetFlag = 0;//0
THtakahiro702286 0:6f4b2005b633 58 float fiveYAcc[5]={0};
THtakahiro702286 0:6f4b2005b633 59
THtakahiro702286 0:6f4b2005b633 60 float servoAngle[3];
THtakahiro702286 27:818783bc9a89 61 float motor=0;
THtakahiro702286 0:6f4b2005b633 62
THtakahiro702286 21:b7734064c7dc 63 for(int i=0;i < 3; i++)servo[i].calibrate(0.00095, 90);
THtakahiro702286 27:818783bc9a89 64 servoAngle[0] = 0.4;
THtakahiro702286 27:818783bc9a89 65 servoAngle[1] = 0.4;
THtakahiro702286 27:818783bc9a89 66 servoAngle[2] = 0.85;
THtakahiro702286 27:818783bc9a89 67 for(i=0;i<3;i++) servo[i].write(servoAngle[i]);
THtakahiro702286 26:7cb2c322153a 68 for(i=0; i<3; i++) light[i] = true;
THtakahiro702286 26:7cb2c322153a 69
THtakahiro702286 16:bdc10f57c11a 70
THtakahiro702286 21:b7734064c7dc 71 wait(5);
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 0:6f4b2005b633 98 //ロータリーエンコーダ
THtakahiro702286 0:6f4b2005b633 99 nowPalse = encoder.getPulses();
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 24:952a01c8baee 106 //加速1段目
THtakahiro702286 24:952a01c8baee 107 if(firstFlag)
THtakahiro702286 0:6f4b2005b633 108 {
THtakahiro702286 24:952a01c8baee 109 motor = FIRST_POWER * way * swingFlag; //振り子を振る、wayで反転を制御
THtakahiro702286 21:b7734064c7dc 110 motor = mSp.path_value(motor);
THtakahiro702286 20:c51583296f7e 111 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
THtakahiro702286 0:6f4b2005b633 112 {
THtakahiro702286 0:6f4b2005b633 113 way *= -1;
THtakahiro702286 24:952a01c8baee 114 nextWait.start();
THtakahiro702286 0:6f4b2005b633 115 setWait.start();
THtakahiro702286 0:6f4b2005b633 116 swingFlag = 0;
THtakahiro702286 25:8bbfa2fa59be 117 if(nextWait > SECOND_LINE) //一定時間経過したらsecondに移行
THtakahiro702286 21:b7734064c7dc 118 {
THtakahiro702286 24:952a01c8baee 119 firstFlag = 0;
THtakahiro702286 24:952a01c8baee 120 secondFlag = 1;
THtakahiro702286 21:b7734064c7dc 121 swingFlag = 0;
THtakahiro702286 24:952a01c8baee 122 nextWait.stop();
THtakahiro702286 24:952a01c8baee 123 nextWait.reset();
THtakahiro702286 21:b7734064c7dc 124 }
THtakahiro702286 0:6f4b2005b633 125 }
THtakahiro702286 24:952a01c8baee 126 if(setWait > FIRST_WAIT)
THtakahiro702286 0:6f4b2005b633 127 {
THtakahiro702286 0:6f4b2005b633 128 setWait.stop();
THtakahiro702286 0:6f4b2005b633 129 setWait.reset();
THtakahiro702286 0:6f4b2005b633 130 swingFlag = 1;
THtakahiro702286 0:6f4b2005b633 131 }
THtakahiro702286 0:6f4b2005b633 132 }
THtakahiro702286 24:952a01c8baee 133
THtakahiro702286 24:952a01c8baee 134 //加速2段目
THtakahiro702286 24:952a01c8baee 135 if(secondFlag)
THtakahiro702286 0:6f4b2005b633 136 {
THtakahiro702286 24:952a01c8baee 137 motor = SECOND_POWER * way * swingFlag; //振り子を振る、wayで反転を制御
THtakahiro702286 24:952a01c8baee 138 nextWait.start();
THtakahiro702286 24:952a01c8baee 139 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
THtakahiro702286 0:6f4b2005b633 140 {
THtakahiro702286 0:6f4b2005b633 141 way *= -1;
THtakahiro702286 24:952a01c8baee 142 setWait.start();
THtakahiro702286 0:6f4b2005b633 143 swingFlag = 0;
THtakahiro702286 25:8bbfa2fa59be 144 if(nextWait > THIRD_LINE) //一定時間経過したらthirdに移行
THtakahiro702286 24:952a01c8baee 145 {
THtakahiro702286 24:952a01c8baee 146 secondFlag = 0;
THtakahiro702286 24:952a01c8baee 147 thirdFlag = 1;
THtakahiro702286 24:952a01c8baee 148 swingFlag = 0;
THtakahiro702286 24:952a01c8baee 149 nextWait.stop();
THtakahiro702286 24:952a01c8baee 150 nextWait.reset();
THtakahiro702286 24:952a01c8baee 151 }
THtakahiro702286 0:6f4b2005b633 152 }
THtakahiro702286 24:952a01c8baee 153 if(setWait > SECOND_WAIT)
THtakahiro702286 24:952a01c8baee 154 {
THtakahiro702286 24:952a01c8baee 155 setWait.stop();
THtakahiro702286 24:952a01c8baee 156 setWait.reset();
THtakahiro702286 24:952a01c8baee 157 swingFlag = 1;
THtakahiro702286 24:952a01c8baee 158 }
THtakahiro702286 0:6f4b2005b633 159 }
THtakahiro702286 21:b7734064c7dc 160
THtakahiro702286 24:952a01c8baee 161 //加速3段目
THtakahiro702286 24:952a01c8baee 162 if(thirdFlag)
THtakahiro702286 0:6f4b2005b633 163 {
THtakahiro702286 24:952a01c8baee 164 jumpWait.start();
THtakahiro702286 24:952a01c8baee 165 motor = THIRD_POWER * way * swingFlag; //振り子を振る、wayで反転を制御
THtakahiro702286 24:952a01c8baee 166 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
THtakahiro702286 24:952a01c8baee 167 {
THtakahiro702286 24:952a01c8baee 168 way *= -1;
THtakahiro702286 24:952a01c8baee 169 nextWait.start();
THtakahiro702286 24:952a01c8baee 170 setWait.start();
THtakahiro702286 24:952a01c8baee 171 swingFlag = 0;
THtakahiro702286 24:952a01c8baee 172 }
THtakahiro702286 24:952a01c8baee 173 if(setWait > THIRD_WAIT)
THtakahiro702286 24:952a01c8baee 174 {
THtakahiro702286 24:952a01c8baee 175 setWait.stop();
THtakahiro702286 24:952a01c8baee 176 setWait.reset();
THtakahiro702286 24:952a01c8baee 177 swingFlag = 1;
THtakahiro702286 24:952a01c8baee 178 }
THtakahiro702286 27:818783bc9a89 179 if((jumpWait > JUMP_TIME) && (nowPalse < JUMP_LINE) && (nowPalse > JUMP_LINE-10) && (way == -1)) //一定時間たったら前に飛ぶ
THtakahiro702286 24:952a01c8baee 180 {
THtakahiro702286 26:7cb2c322153a 181 flyFlag = 1;
THtakahiro702286 26:7cb2c322153a 182 thirdFlag = 0;
THtakahiro702286 24:952a01c8baee 183 }
THtakahiro702286 0:6f4b2005b633 184 }
THtakahiro702286 27:818783bc9a89 185
THtakahiro702286 27:818783bc9a89 186
THtakahiro702286 0:6f4b2005b633 187 //ジャンプ
THtakahiro702286 0:6f4b2005b633 188 if(flyFlag)
THtakahiro702286 0:6f4b2005b633 189 {
THtakahiro702286 27:818783bc9a89 190 // if(lpfZ < RELEASE_LINE)
THtakahiro702286 0:6f4b2005b633 191 {
THtakahiro702286 27:818783bc9a89 192 servoAngle[(k+1)%3] = 0.0;//サーボ離す
THtakahiro702286 27:818783bc9a89 193 servoAngle[k%3] = 0.85;
THtakahiro702286 27:818783bc9a89 194 k+=2;
THtakahiro702286 27:818783bc9a89 195 l++;
THtakahiro702286 0:6f4b2005b633 196 flyFlag = 0;
THtakahiro702286 27:818783bc9a89 197 catchFlag=1;
THtakahiro702286 27:818783bc9a89 198 }
THtakahiro702286 27:818783bc9a89 199 motor=0;
THtakahiro702286 27:818783bc9a89 200
THtakahiro702286 0:6f4b2005b633 201 }
THtakahiro702286 27:818783bc9a89 202
THtakahiro702286 27:818783bc9a89 203
THtakahiro702286 0:6f4b2005b633 204 //キャッチ
THtakahiro702286 0:6f4b2005b633 205 if(catchFlag)
THtakahiro702286 0:6f4b2005b633 206 {
THtakahiro702286 27:818783bc9a89 207 if(nowPalse < NEXT_PALSE * l)
THtakahiro702286 27:818783bc9a89 208 {
THtakahiro702286 27:818783bc9a89 209 // motor=0.95;
THtakahiro702286 27:818783bc9a89 210 }
THtakahiro702286 27:818783bc9a89 211 else
THtakahiro702286 27:818783bc9a89 212 {
THtakahiro702286 27:818783bc9a89 213 // motor=0;
THtakahiro702286 27:818783bc9a89 214 }
THtakahiro702286 27:818783bc9a89 215 if(phI[k%3] < ROPE_ACRROS[k%3])
THtakahiro702286 27:818783bc9a89 216 {
THtakahiro702286 27:818783bc9a89 217 catchFlag=0;
THtakahiro702286 27:818783bc9a89 218 motor = 0;
THtakahiro702286 27:818783bc9a89 219 }
THtakahiro702286 27:818783bc9a89 220
THtakahiro702286 0:6f4b2005b633 221 }
THtakahiro702286 27:818783bc9a89 222 /*
THtakahiro702286 0:6f4b2005b633 223 //掴んだ後
THtakahiro702286 0:6f4b2005b633 224 if(resetFlag)
THtakahiro702286 0:6f4b2005b633 225 {
THtakahiro702286 0:6f4b2005b633 226 resetFlag = 0;
THtakahiro702286 24:952a01c8baee 227 secondFlag = 1;
THtakahiro702286 0:6f4b2005b633 228 }
THtakahiro702286 21:b7734064c7dc 229 */
THtakahiro702286 14:7e5b21c812fd 230 //実験用記述-----------
THtakahiro702286 27:818783bc9a89 231 /*
THtakahiro702286 22:efa233983e1f 232 if(flyFlag)
THtakahiro702286 22:efa233983e1f 233 {
THtakahiro702286 22:efa233983e1f 234 // if(lpfZ < RELEASELINE)
THtakahiro702286 22:efa233983e1f 235 {
THtakahiro702286 22:efa233983e1f 236 for(i=0; i < 3; i++) servoAngle[i] = -1.0;//サーボ離す
THtakahiro702286 22:efa233983e1f 237 // k++;
THtakahiro702286 22:efa233983e1f 238 flyFlag = 0;
THtakahiro702286 22:efa233983e1f 239 }
THtakahiro702286 22:efa233983e1f 240 motor=0;
THtakahiro702286 22:efa233983e1f 241 }
THtakahiro702286 27:818783bc9a89 242 */
THtakahiro702286 27:818783bc9a89 243 /*
THtakahiro702286 27:818783bc9a89 244 for(i=0; i<3; i++)
THtakahiro702286 27:818783bc9a89 245 {
THtakahiro702286 27:818783bc9a89 246 if(phI[i] < ROPE_ACRROS[i]) servoAngle[i] = -0.8;
THtakahiro702286 27:818783bc9a89 247
THtakahiro702286 27:818783bc9a89 248 }
THtakahiro702286 27:818783bc9a89 249 */
THtakahiro702286 27:818783bc9a89 250 /*
THtakahiro702286 27:818783bc9a89 251 if(phI[k%3] < ROPE_ACRROS[k%3])
THtakahiro702286 27:818783bc9a89 252 {
THtakahiro702286 27:818783bc9a89 253 servoAngle[k%3] = 0.85;
THtakahiro702286 27:818783bc9a89 254 servoAngle[(k+1)%3] = -0.5;
THtakahiro702286 27:818783bc9a89 255 k+=2;
THtakahiro702286 27:818783bc9a89 256 }
THtakahiro702286 27:818783bc9a89 257 */
THtakahiro702286 27:818783bc9a89 258 // if(!b) motor=0.3;
THtakahiro702286 27:818783bc9a89 259 // else motor=0;
THtakahiro702286 22:efa233983e1f 260
THtakahiro702286 14:7e5b21c812fd 261 //-------------------
umekou 12:915a390e3fc4 262
THtakahiro702286 0:6f4b2005b633 263 //駆動系
umekou 12:915a390e3fc4 264
THtakahiro702286 0:6f4b2005b633 265 //サーボ
THtakahiro702286 0:6f4b2005b633 266 for(i=0;i<3;i++) servo[i].write(servoAngle[i]);
THtakahiro702286 0:6f4b2005b633 267
THtakahiro702286 0:6f4b2005b633 268 //モーター
THtakahiro702286 20:c51583296f7e 269 ir2302.SetMotorSpeed(motor);
THtakahiro702286 18:20dee7c7a628 270
THtakahiro702286 15:d8db04a67df9 271 //表示系
THtakahiro702286 18:20dee7c7a628 272 // pc.printf("%.2f,%.2f,%.2f",angle[0],angle[1],angle[2]);
THtakahiro702286 24:952a01c8baee 273 // pc.printf("%.2f,%.2f,%.2f,",servoAngle[0],servoAngle[1],servoAngle[2]);
THtakahiro702286 24:952a01c8baee 274 // pc.printf("%.4f,",motor);
THtakahiro702286 18:20dee7c7a628 275 // pc.printf("%.2f,%.2f,",abac[1],abac[2]);
THtakahiro702286 22:efa233983e1f 276 // pc.printf("%.2f,%.2f,",lpfY,lpfZ);
THtakahiro702286 19:b9f53bc0fdf2 277 // pc.printf("%d,",nowPalse);
THtakahiro702286 27:818783bc9a89 278 // pc.printf("%d,",catchFlag);
THtakahiro702286 21:b7734064c7dc 279 // pc.printf("%.2f,",lpfM);
THtakahiro702286 27:818783bc9a89 280 // pc.printf("%.3f,%.3f,%.3f,",phI[0],phI[1],phI[2]);
THtakahiro702286 27:818783bc9a89 281 // pc.printf("\r\n");
THtakahiro702286 18:20dee7c7a628 282
THtakahiro702286 0:6f4b2005b633 283 }
THtakahiro702286 0:6f4b2005b633 284 }
umekou 12:915a390e3fc4 285
umekou 12:915a390e3fc4 286