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@26:7cb2c322153a, 2021-10-23 (annotated)

Committer:
THtakahiro702286
Date:
Sat Oct 23 05:43:25 2021 +0000
Revision:
26:7cb2c322153a
Parent:
25:8bbfa2fa59be
Child:
27:818783bc9a89
no catch

Who changed what in which revision?

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