Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@5:fcc79e507610, 2019-08-24 (annotated)

Committer:
Tom0108
Date:
Sat Aug 24 00:30:20 2019 +0000
Revision:
5:fcc79e507610
Parent:
4:236e5c58e8ee
Child:
6:040d001acb12
You can use a gyro

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tom0108 0:761a63c6d020 1 /*今日 やること*/
Tom0108 0:761a63c6d020 2
Tom0108 3:1063aa133b62 3 /******************************************************************************/
Tom0108 0:761a63c6d020 4 #include "main.h"
Tom0108 0:761a63c6d020 5 //#include "gyro.h"
Tom0108 0:761a63c6d020 6 #include <math.h>
Tom0108 0:761a63c6d020 7
Tom0108 3:1063aa133b62 8 /******************************************************************************/
Tom0108 0:761a63c6d020 9 #define CIRCLE sbdbt.CIRCLE
Tom0108 0:761a63c6d020 10 #define SQUARE sbdbt.SQUARE
Tom0108 0:761a63c6d020 11 #define CROSS sbdbt.CROSS
Tom0108 0:761a63c6d020 12 #define TRIANGLE sbdbt.TRIANGLE
Tom0108 0:761a63c6d020 13 #define LEFTkey sbdbt.LEFTkey
Tom0108 0:761a63c6d020 14 #define RIGHTkey sbdbt.RIGHTkey
Tom0108 0:761a63c6d020 15 #define UPkey sbdbt.UPkey
Tom0108 0:761a63c6d020 16 #define DOWNkey sbdbt.DOWNkey
Tom0108 0:761a63c6d020 17
Tom0108 3:1063aa133b62 18 /******************************************************************************/
Tom0108 0:761a63c6d020 19
Tom0108 0:761a63c6d020 20 /* 型定義 --------------------------------------------------------------------*/
Tom0108 0:761a63c6d020 21
Tom0108 0:761a63c6d020 22 MD_I2C_Data_TypeDef MD_Data[Motor_MAX] = {
Tom0108 0:761a63c6d020 23 { 8, 0, 0, 0, {0}}, //足回り
Tom0108 0:761a63c6d020 24 { 9, 1, 0, 0, {0}},
Tom0108 0:761a63c6d020 25 {10, 0, 0, 0, {0}},
Tom0108 0:761a63c6d020 26 {11, 1, 0, 0, {0}},
Tom0108 0:761a63c6d020 27 {12, 0, 0, 0, {0}}, // 洗濯物回収
Tom0108 0:761a63c6d020 28 {13, 1, 0, 0, {0}}, // 上下機構
Tom0108 0:761a63c6d020 29 {14, 0, 0, 0, {0}}, // 洗濯物排出
Tom0108 0:761a63c6d020 30 {15, 1, 0, 0, {0}} // 左右移動
Tom0108 0:761a63c6d020 31 };
Tom0108 0:761a63c6d020 32
Tom0108 3:1063aa133b62 33 IN_I2C_Data_TypeDef In_Data[] = {
Tom0108 0:761a63c6d020 34 {1,0,0}, // 洗濯物排出(初期位置)
Tom0108 0:761a63c6d020 35 {1,1,0}, // 洗濯物排出(停止)
Tom0108 0:761a63c6d020 36 {1,2,0}, // 腕(最大 タオル)
Tom0108 3:1063aa133b62 37 {1,3,0}, // 腕(初期位置)
Tom0108 0:761a63c6d020 38 {3,0,0}, // 上下機構(上)
Tom0108 0:761a63c6d020 39 {3,1,0}, // (下)
Tom0108 0:761a63c6d020 40 {3,2,0}, // 腕(シーツ兼シャツ）
Tom0108 0:761a63c6d020 41 };
Tom0108 0:761a63c6d020 42
Tom0108 0:761a63c6d020 43 OUT_I2C_Data_TypeDef Out_Data[] = {
Tom0108 0:761a63c6d020 44 {0,0,0}, // 腕の上下
Tom0108 2:47954f05d32d 45 {0,1,0}, // 爪
Tom0108 2:47954f05d32d 46 {0,2,0}, // タオルの先
Tom0108 2:47954f05d32d 47 {0,3,0}, // タオル展開
Tom0108 2:47954f05d32d 48 {1,0,0}, // タオルの中央
Tom0108 0:761a63c6d020 49 };
Tom0108 0:761a63c6d020 50 /* 関数宣言 ------------------------------------------------------------------*/
Tom0108 0:761a63c6d020 51 void mecanum_Move(void);
Tom0108 0:761a63c6d020 52 void mecanum_Turn(void);
Tom0108 0:761a63c6d020 53 void mecanum_Stop(void);
Tom0108 0:761a63c6d020 54
Tom0108 0:761a63c6d020 55 /* 変数宣言 ------------------------------------------------------------------*/
Tom0108 1:199c4a71da88 56 //かご回転用
Tom0108 3:1063aa133b62 57 int kago=0, kago_f=0;
Tom0108 1:199c4a71da88 58
Tom0108 1:199c4a71da88 59 //コントローラーの状態を保存するため
Tom0108 1:199c4a71da88 60 int once=0;
Tom0108 1:199c4a71da88 61
Tom0108 1:199c4a71da88 62 //タオル引く用
Tom0108 3:1063aa133b62 63 int pull=0, pull_f=0;
Tom0108 1:199c4a71da88 64
Tom0108 4:236e5c58e8ee 65 //タオル掴む用
Tom0108 4:236e5c58e8ee 66 //sgrab_f: 中央のリミットでつかむ
Tom0108 4:236e5c58e8ee 67 int grab=0, grab_f=0, sgrab_f=0;
Tom0108 3:1063aa133b62 68
Tom0108 4:236e5c58e8ee 69 //タオル横展開用
Tom0108 3:1063aa133b62 70 int unfold_f=0;
Tom0108 3:1063aa133b62 71
Tom0108 3:1063aa133b62 72 //90度回転のフラグ
Tom0108 4:236e5c58e8ee 73 int left_f=0, right_f=0;
Tom0108 1:199c4a71da88 74
Tom0108 3:1063aa133b62 75 //y_deg: 起動時から絶対の角度
Tom0108 3:1063aa133b62 76 //yaw: 相対角 初期状態が0になるようにする
Tom0108 3:1063aa133b62 77 //offset: 初期状態を0にするために引く値
Tom0108 5:fcc79e507610 78 double y_deg[2], offset;
Tom0108 5:fcc79e507610 79 double yaw=0;
Tom0108 5:fcc79e507610 80 double sum=0, ave=0;
Tom0108 5:fcc79e507610 81 int offset_cnt=0;
Tom0108 3:1063aa133b62 82
Tom0108 3:1063aa133b62 83 //Turn_val: 補正の目標値
Tom0108 3:1063aa133b62 84 //dif_val: 補正値との差
Tom0108 3:1063aa133b62 85 double Turn_val, dif_val;
Tom0108 3:1063aa133b62 86
Tom0108 3:1063aa133b62 87 //short_lim: 腕中央
Tom0108 3:1063aa133b62 88 //max_lim: 腕奥側
Tom0108 3:1063aa133b62 89 //short_lim: 腕手前側
Tom0108 3:1063aa133b62 90 int short_lim, max_lim, start_lim;
Tom0108 1:199c4a71da88 91
Tom0108 4:236e5c58e8ee 92 //機体の前方を変更する
Tom0108 4:236e5c58e8ee 93 int dir=0;
Tom0108 4:236e5c58e8ee 94
Tom0108 0:761a63c6d020 95 int limit_up,limit_down; // リミットスイッチ
Tom0108 1:199c4a71da88 96 int slide_start,slide_stop; //かごのリミット
Tom0108 0:761a63c6d020 97
Tom0108 0:761a63c6d020 98 int lx,ly,rx,L,R; // 足回り
Tom0108 0:761a63c6d020 99 int g;
Tom0108 0:761a63c6d020 100
Tom0108 0:761a63c6d020 101 int as=0;
Tom0108 0:761a63c6d020 102
Tom0108 0:761a63c6d020 103 /*クラス宣言-------------------------------------------------------------------*/
Tom0108 0:761a63c6d020 104 Ticker flipper1; // 割込み
Tom0108 0:761a63c6d020 105 Y_I2C i2c(PB_9, PB_8); // I2C
Tom0108 0:761a63c6d020 106 DigitalOut myled(LED1); // オンボードLED
Tom0108 0:761a63c6d020 107 SBDBT sbdbt(PA_0, PA_1, 9600); // SDBDT
Tom0108 0:761a63c6d020 108 MyMPU6050 mpu(PC_9, PA_8); // ジャイロセンサ
Tom0108 0:761a63c6d020 109
Tom0108 0:761a63c6d020 110 DigitalOut blue(PA_10);
Tom0108 0:761a63c6d020 111 DigitalOut green(PB_4);
Tom0108 0:761a63c6d020 112 DigitalOut red(PB_5);
Tom0108 0:761a63c6d020 113
Tom0108 3:1063aa133b62 114 //半自動の時に使う
Tom0108 3:1063aa133b62 115 Timer tim;
Tom0108 3:1063aa133b62 116
Tom0108 0:761a63c6d020 117 /*----------------------------------- main -----------------------------------*/
Tom0108 0:761a63c6d020 118 /* @brief メインプログラム
Tom0108 0:761a63c6d020 119 * @param なし
Tom0108 0:761a63c6d020 120 * @retval なし
Tom0108 0:761a63c6d020 121 */
Tom0108 0:761a63c6d020 122 int main()
Tom0108 0:761a63c6d020 123 {
Tom0108 0:761a63c6d020 124 //flipper1.attach(&flip1, 0.001); // 割り込み
Tom0108 5:fcc79e507610 125
Tom0108 0:761a63c6d020 126 while(1) {
Tom0108 1:199c4a71da88 127 //自動系のタイマースタート
Tom0108 1:199c4a71da88 128 tim.start();
Tom0108 0:761a63c6d020 129 mpu.loop();
Tom0108 0:761a63c6d020 130
Tom0108 5:fcc79e507610 131 y_deg[1] = y_deg[0];
Tom0108 5:fcc79e507610 132 y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI;
Tom0108 5:fcc79e507610 133
Tom0108 5:fcc79e507610 134 //180～-180をまたいだ時
Tom0108 5:fcc79e507610 135 if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360;
Tom0108 5:fcc79e507610 136 else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360;
Tom0108 0:761a63c6d020 137
Tom0108 5:fcc79e507610 138 if(sbdbt.State_check()) {
Tom0108 5:fcc79e507610 139 for(int i=0; i<7; i++) i2c.In(In_Data,i);
Tom0108 5:fcc79e507610 140
Tom0108 5:fcc79e507610 141 //誤差をさんぷりんぐ
Tom0108 5:fcc79e507610 142 if(offset_cnt<1000) {
Tom0108 5:fcc79e507610 143 if(offset_cnt!=0) {
Tom0108 5:fcc79e507610 144 sum+=(y_deg[0]-y_deg[1]);
Tom0108 5:fcc79e507610 145 ave=sum/offset_cnt;
Tom0108 5:fcc79e507610 146 }
Tom0108 5:fcc79e507610 147 offset_cnt++;
Tom0108 5:fcc79e507610 148 } else yaw += (y_deg[0]-y_deg[1])-ave;
Tom0108 5:fcc79e507610 149 // yaw=90*dir;
Tom0108 5:fcc79e507610 150 // yaw%=360;
Tom0108 5:fcc79e507610 151 dif_val = y_deg[0]-Turn_val;
Tom0108 0:761a63c6d020 152
Tom0108 0:761a63c6d020 153 lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180);
Tom0108 0:761a63c6d020 154 ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180);
Tom0108 0:761a63c6d020 155 rx = (sbdbt.RX - 64)*100/64;
Tom0108 0:761a63c6d020 156 L = lx+ly;
Tom0108 0:761a63c6d020 157 R = ly-lx;
Tom0108 0:761a63c6d020 158
Tom0108 0:761a63c6d020 159 if(sbdbt.LX !=64 || sbdbt.LY !=64) {
Tom0108 0:761a63c6d020 160
Tom0108 0:761a63c6d020 161 mecanum_Move();
Tom0108 0:761a63c6d020 162
Tom0108 0:761a63c6d020 163 // 以下，ジャイロセンサーのプログラム
Tom0108 0:761a63c6d020 164 if(g) {
Tom0108 0:761a63c6d020 165 if(abs(ly) > 10) {
Tom0108 0:761a63c6d020 166 if(ly>10) {
Tom0108 0:761a63c6d020 167 (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
Tom0108 0:761a63c6d020 168 (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)+dif_val;
Tom0108 0:761a63c6d020 169 } else {
Tom0108 0:761a63c6d020 170 (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
Tom0108 0:761a63c6d020 171 (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)-dif_val;
Tom0108 0:761a63c6d020 172 }
Tom0108 0:761a63c6d020 173 }
Tom0108 0:761a63c6d020 174
Tom0108 0:761a63c6d020 175 if(abs(lx) > 10) {
Tom0108 0:761a63c6d020 176 if(lx>10) {
Tom0108 0:761a63c6d020 177 (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
Tom0108 0:761a63c6d020 178 (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)+dif_val;
Tom0108 0:761a63c6d020 179 } else {
Tom0108 0:761a63c6d020 180 (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
Tom0108 0:761a63c6d020 181 (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)-dif_val;
Tom0108 0:761a63c6d020 182 }
Tom0108 0:761a63c6d020 183 }
Tom0108 0:761a63c6d020 184
Tom0108 0:761a63c6d020 185 if((abs(ly)-abs(lx)) > 0) {
Tom0108 0:761a63c6d020 186 (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,0);
Tom0108 0:761a63c6d020 187 (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,1);
Tom0108 0:761a63c6d020 188 } else {
Tom0108 0:761a63c6d020 189 (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,0);
Tom0108 0:761a63c6d020 190 (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,2);
Tom0108 0:761a63c6d020 191 }
Tom0108 0:761a63c6d020 192 } // if(g)
Tom0108 0:761a63c6d020 193 }// if(LX !=64 || LY !=64)
Tom0108 0:761a63c6d020 194 else if(sbdbt.RX != 64) { // 旋回
Tom0108 5:fcc79e507610 195 Turn_val=y_deg[0];
Tom0108 0:761a63c6d020 196 mecanum_Turn();
Tom0108 0:761a63c6d020 197 } else {
Tom0108 0:761a63c6d020 198 mecanum_Stop();
Tom0108 0:761a63c6d020 199 }
Tom0108 3:1063aa133b62 200
Tom0108 3:1063aa133b62 201 // 90°旋回
Tom0108 3:1063aa133b62 202 if(left_f) {
Tom0108 0:761a63c6d020 203 if(abs(yaw)<60) {
Tom0108 0:761a63c6d020 204 for(int i=0; i<4; i++) {
Tom0108 0:761a63c6d020 205 MD_SET_PWM(MD_Data, i,60);
Tom0108 0:761a63c6d020 206 MD_SET_DRIVE(MD_Data, i,MD_REVERSE);
Tom0108 0:761a63c6d020 207 }
Tom0108 0:761a63c6d020 208 } else {
Tom0108 0:761a63c6d020 209 left_f=0;
Tom0108 5:fcc79e507610 210 Turn_val=y_deg[0];
Tom0108 0:761a63c6d020 211 mecanum_Stop();
Tom0108 0:761a63c6d020 212 }
Tom0108 0:761a63c6d020 213 }
Tom0108 0:761a63c6d020 214 if(right_f) {
Tom0108 0:761a63c6d020 215 if(yaw>-60) {
Tom0108 0:761a63c6d020 216 for(int i=0; i<4; i++) {
Tom0108 0:761a63c6d020 217 MD_SET_PWM(MD_Data, i,60);
Tom0108 0:761a63c6d020 218 MD_SET_DRIVE(MD_Data, i,MD_FORWARD);
Tom0108 0:761a63c6d020 219 }
Tom0108 0:761a63c6d020 220 } else {
Tom0108 0:761a63c6d020 221 right_f=0;
Tom0108 5:fcc79e507610 222 Turn_val=y_deg[0];
Tom0108 0:761a63c6d020 223 mecanum_Stop();
Tom0108 0:761a63c6d020 224 }
Tom0108 0:761a63c6d020 225 }
Tom0108 0:761a63c6d020 226
Tom0108 3:1063aa133b62 227 /*---ハンガーかけるやつ----------------------------------------------*/
Tom0108 1:199c4a71da88 228 if(sbdbt.SELECT)as=1;//selectキー
Tom0108 0:761a63c6d020 229
Tom0108 0:761a63c6d020 230 if(as==1 && limit_up == 0) {
Tom0108 0:761a63c6d020 231 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 0:761a63c6d020 232 MD_SET_PWM(MD_Data, 5,50);
Tom0108 0:761a63c6d020 233 } else if(UPkey && limit_up == 0) {
Tom0108 0:761a63c6d020 234 as=0;
Tom0108 0:761a63c6d020 235 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 0:761a63c6d020 236 MD_SET_PWM(MD_Data, 5,50);
Tom0108 0:761a63c6d020 237 } else if(as==1 && limit_up == 1) {
Tom0108 0:761a63c6d020 238 as=0;
Tom0108 0:761a63c6d020 239 MD_SET_PWM(MD_Data,5,0);
Tom0108 0:761a63c6d020 240 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 0:761a63c6d020 241 } else if(DOWNkey && limit_down == 0) {
Tom0108 0:761a63c6d020 242 MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
Tom0108 0:761a63c6d020 243 MD_SET_PWM(MD_Data, 5,50);
Tom0108 0:761a63c6d020 244 } else {
Tom0108 0:761a63c6d020 245 MD_SET_PWM(MD_Data,5,0);
Tom0108 0:761a63c6d020 246 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 0:761a63c6d020 247 }
Tom0108 0:761a63c6d020 248
Tom0108 1:199c4a71da88 249 //タオルつかむ 四角
Tom0108 4:236e5c58e8ee 250 if(grab_f || sgrab_f) {
Tom0108 1:199c4a71da88 251 switch(grab) {
Tom0108 1:199c4a71da88 252 //腕を前に動かす
Tom0108 1:199c4a71da88 253 case 0:
Tom0108 1:199c4a71da88 254 //爪開く
Tom0108 1:199c4a71da88 255 i2c.Out_Set(Out_Data,1,1);
Tom0108 1:199c4a71da88 256 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 4:236e5c58e8ee 257 MD_SET_PWM(MD_Data,4,40);
Tom0108 4:236e5c58e8ee 258 if((sgrab_f && short_lim) || max_lim==1) {
Tom0108 1:199c4a71da88 259 grab++;
Tom0108 1:199c4a71da88 260 tim.reset();
Tom0108 1:199c4a71da88 261 }
Tom0108 1:199c4a71da88 262 break;
Tom0108 0:761a63c6d020 263
Tom0108 1:199c4a71da88 264 case 1:
Tom0108 2:47954f05d32d 265 //腕停止
Tom0108 2:47954f05d32d 266 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 3:1063aa133b62 267 MD_SET_PWM(MD_Data,4,0);
Tom0108 2:47954f05d32d 268
Tom0108 1:199c4a71da88 269 //腕おろす
Tom0108 0:761a63c6d020 270 i2c.Out_Set(Out_Data,0,1);
Tom0108 3:1063aa133b62 271 if(tim.read_ms()>300) {
Tom0108 1:199c4a71da88 272 grab++;
Tom0108 1:199c4a71da88 273 tim.reset();
Tom0108 1:199c4a71da88 274 }
Tom0108 1:199c4a71da88 275 break;
Tom0108 0:761a63c6d020 276
Tom0108 1:199c4a71da88 277 case 2:
Tom0108 1:199c4a71da88 278 //爪閉じる
Tom0108 1:199c4a71da88 279 i2c.Out_Set(Out_Data,1,0);
Tom0108 3:1063aa133b62 280 if(tim.read_ms()>100) {
Tom0108 1:199c4a71da88 281 grab++;
Tom0108 0:761a63c6d020 282 tim.reset();
Tom0108 0:761a63c6d020 283 }
Tom0108 1:199c4a71da88 284 break;
Tom0108 1:199c4a71da88 285
Tom0108 1:199c4a71da88 286 case 3:
Tom0108 1:199c4a71da88 287 //腕上げる
Tom0108 1:199c4a71da88 288 i2c.Out_Set(Out_Data,0,0);
Tom0108 3:1063aa133b62 289 if(tim.read_ms()>300) {
Tom0108 3:1063aa133b62 290 grab++;
Tom0108 3:1063aa133b62 291 tim.reset();
Tom0108 3:1063aa133b62 292 }
Tom0108 3:1063aa133b62 293 break;
Tom0108 3:1063aa133b62 294
Tom0108 3:1063aa133b62 295 case 4:
Tom0108 1:199c4a71da88 296 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 3:1063aa133b62 297 MD_SET_PWM (MD_Data,4,60);
Tom0108 4:236e5c58e8ee 298 if(start_lim==1) {
Tom0108 4:236e5c58e8ee 299 grab_f=0;
Tom0108 4:236e5c58e8ee 300 sgrab_f=0;
Tom0108 4:236e5c58e8ee 301 }
Tom0108 1:199c4a71da88 302 break;
Tom0108 0:761a63c6d020 303 }
Tom0108 3:1063aa133b62 304 } //if(grab_f)
Tom0108 3:1063aa133b62 305
Tom0108 2:47954f05d32d 306 //タオル引く 丸
Tom0108 3:1063aa133b62 307 else if(pull_f) {
Tom0108 1:199c4a71da88 308 switch(pull) {
Tom0108 1:199c4a71da88 309 //腕を前に動かす
Tom0108 1:199c4a71da88 310 case 0:
Tom0108 1:199c4a71da88 311 //爪閉じる
Tom0108 1:199c4a71da88 312 i2c.Out_Set(Out_Data,1,0);
Tom0108 0:761a63c6d020 313 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 3:1063aa133b62 314 MD_SET_PWM (MD_Data,4,60);
Tom0108 1:199c4a71da88 315 if(max_lim==1) {
Tom0108 1:199c4a71da88 316 pull++;
Tom0108 1:199c4a71da88 317 tim.reset();
Tom0108 1:199c4a71da88 318 }
Tom0108 1:199c4a71da88 319 break;
Tom0108 0:761a63c6d020 320
Tom0108 1:199c4a71da88 321 case 1:
Tom0108 2:47954f05d32d 322 //腕停止
Tom0108 2:47954f05d32d 323 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 2:47954f05d32d 324 MD_SET_PWM (MD_Data,4,0);
Tom0108 2:47954f05d32d 325
Tom0108 1:199c4a71da88 326 //腕おろす
Tom0108 0:761a63c6d020 327 i2c.Out_Set(Out_Data,0,1);
Tom0108 3:1063aa133b62 328 if(tim.read_ms()>300) {
Tom0108 1:199c4a71da88 329 pull++;
Tom0108 1:199c4a71da88 330 tim.reset();
Tom0108 0:761a63c6d020 331 }
Tom0108 1:199c4a71da88 332 break;
Tom0108 1:199c4a71da88 333
Tom0108 1:199c4a71da88 334 case 2:
Tom0108 3:1063aa133b62 335 if(tim.read_ms()>300) {
Tom0108 1:199c4a71da88 336 pull++;
Tom0108 0:761a63c6d020 337 tim.reset();
Tom0108 0:761a63c6d020 338 }
Tom0108 1:199c4a71da88 339 break;
Tom0108 1:199c4a71da88 340
Tom0108 1:199c4a71da88 341 case 3:
Tom0108 1:199c4a71da88 342 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 3:1063aa133b62 343 MD_SET_PWM (MD_Data,4,60);
Tom0108 3:1063aa133b62 344 if(start_lim==1) pull_f=0;
Tom0108 1:199c4a71da88 345 break;
Tom0108 0:761a63c6d020 346 }
Tom0108 3:1063aa133b62 347 } //if(pull_f)
Tom0108 3:1063aa133b62 348
Tom0108 3:1063aa133b62 349 //かごを倒す
Tom0108 3:1063aa133b62 350 else if(kago_f) {
Tom0108 1:199c4a71da88 351 //腕の先を閉じておく
Tom0108 1:199c4a71da88 352 i2c.Out_Set(Out_Data,1,0);
Tom0108 0:761a63c6d020 353
Tom0108 1:199c4a71da88 354 switch(kago) {
Tom0108 1:199c4a71da88 355 //腕を前に動かす
Tom0108 1:199c4a71da88 356 case 0:
Tom0108 0:761a63c6d020 357 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 3:1063aa133b62 358 MD_SET_PWM (MD_Data,4,60);
Tom0108 2:47954f05d32d 359 if(max_lim==1) {
Tom0108 2:47954f05d32d 360 kago++;
Tom0108 2:47954f05d32d 361 tim.reset();
Tom0108 2:47954f05d32d 362 }
Tom0108 1:199c4a71da88 363 break;
Tom0108 1:199c4a71da88 364
Tom0108 1:199c4a71da88 365 case 1:
Tom0108 1:199c4a71da88 366 //腕停止
Tom0108 0:761a63c6d020 367 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 1:199c4a71da88 368 MD_SET_PWM (MD_Data,4,0);
Tom0108 0:761a63c6d020 369
Tom0108 1:199c4a71da88 370 //かご回転
Tom0108 1:199c4a71da88 371 MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
Tom0108 3:1063aa133b62 372 MD_SET_PWM (MD_Data,6,80);
Tom0108 2:47954f05d32d 373 if(slide_stop==1 || tim.read_ms()>3000) {
Tom0108 1:199c4a71da88 374 kago++;
Tom0108 2:47954f05d32d 375 tim.reset();
Tom0108 1:199c4a71da88 376 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 1:199c4a71da88 377 MD_SET_PWM (MD_Data,6,0);
Tom0108 1:199c4a71da88 378 }
Tom0108 1:199c4a71da88 379 break;
Tom0108 0:761a63c6d020 380
Tom0108 1:199c4a71da88 381 case 2:
Tom0108 3:1063aa133b62 382 if(tim.read_ms()>1000) {
Tom0108 2:47954f05d32d 383 kago++;
Tom0108 2:47954f05d32d 384 tim.reset();
Tom0108 2:47954f05d32d 385 }
Tom0108 1:199c4a71da88 386 break;
Tom0108 1:199c4a71da88 387
Tom0108 1:199c4a71da88 388 case 3:
Tom0108 1:199c4a71da88 389 MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
Tom0108 3:1063aa133b62 390 MD_SET_PWM (MD_Data,6,80);
Tom0108 2:47954f05d32d 391 if(slide_start==1 || tim.read_ms()>3000) kago++;
Tom0108 1:199c4a71da88 392 break;
Tom0108 1:199c4a71da88 393
Tom0108 1:199c4a71da88 394 case 4:
Tom0108 1:199c4a71da88 395 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 3:1063aa133b62 396 MD_SET_PWM (MD_Data,4,60);
Tom0108 3:1063aa133b62 397 if(start_lim==1) kago_f=0;
Tom0108 1:199c4a71da88 398 break;
Tom0108 0:761a63c6d020 399 }
Tom0108 0:761a63c6d020 400 } else {
Tom0108 3:1063aa133b62 401 //かごと腕を停止
Tom0108 0:761a63c6d020 402 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 1:199c4a71da88 403 MD_SET_PWM (MD_Data,4,0);
Tom0108 1:199c4a71da88 404 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 1:199c4a71da88 405 MD_SET_PWM (MD_Data,6,0);
Tom0108 3:1063aa133b62 406
Tom0108 1:199c4a71da88 407 //爪開く
Tom0108 2:47954f05d32d 408 i2c.Out_Set(Out_Data,1,1);
Tom0108 1:199c4a71da88 409 //腕上げる
Tom0108 2:47954f05d32d 410 i2c.Out_Set(Out_Data,0,0);
Tom0108 2:47954f05d32d 411
Tom0108 1:199c4a71da88 412 kago=0;
Tom0108 1:199c4a71da88 413 pull=0;
Tom0108 1:199c4a71da88 414 grab=0;
Tom0108 3:1063aa133b62 415
Tom0108 2:47954f05d32d 416 tim.reset();
Tom0108 0:761a63c6d020 417 }
Tom0108 0:761a63c6d020 418
Tom0108 3:1063aa133b62 419 //-------------- 出力 --------------//
Tom0108 3:1063aa133b62 420 //第二引数には使う最大の個数
Tom0108 3:1063aa133b62 421 i2c.Out(Out_Data,5);
Tom0108 3:1063aa133b62 422
Tom0108 3:1063aa133b62 423 //モータ出力
Tom0108 3:1063aa133b62 424 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
Tom0108 3:1063aa133b62 425
Tom0108 3:1063aa133b62 426 /* ----------- りみっと --------------- */
Tom0108 3:1063aa133b62 427 limit_up = (In_Data+4)->in_data;
Tom0108 3:1063aa133b62 428 limit_down = (In_Data+5)->in_data;
Tom0108 3:1063aa133b62 429 max_lim = (In_Data+1)->in_data;
Tom0108 3:1063aa133b62 430 start_lim =(In_Data+2)->in_data;
Tom0108 3:1063aa133b62 431 short_lim = (In_Data+6)->in_data;
Tom0108 3:1063aa133b62 432 slide_start = (In_Data+3)->in_data;
Tom0108 3:1063aa133b62 433 slide_stop = (In_Data+0)->in_data;
Tom0108 3:1063aa133b62 434
Tom0108 3:1063aa133b62 435 //半自動のデバック
Tom0108 3:1063aa133b62 436 // pc.printf("pull %2d%2d ",pull,pull_f);
Tom0108 3:1063aa133b62 437 // pc.printf("grab %2d%2d ",grab,grab_f);
Tom0108 3:1063aa133b62 438 // pc.printf("kago %2d%2d ",kago,kago_f);
Tom0108 5:fcc79e507610 439 pc.printf("0:%f 1:%f ave:%f yaw:%f",y_deg[0],y_deg[1],ave,yaw);
Tom0108 4:236e5c58e8ee 440 // pc.printf("%2d ",dir);
Tom0108 3:1063aa133b62 441 pc.printf("\n\r");
Tom0108 3:1063aa133b62 442
Tom0108 3:1063aa133b62 443
Tom0108 3:1063aa133b62 444 /* --------------電磁弁系-------------------- */
Tom0108 0:761a63c6d020 445 /*タオル掛け*/
Tom0108 0:761a63c6d020 446 if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
Tom0108 0:761a63c6d020 447 else i2c.Out_Set(Out_Data,2,0);
Tom0108 3:1063aa133b62 448
Tom0108 0:761a63c6d020 449 /*タオルを引っ張るやつ*/
Tom0108 1:199c4a71da88 450 if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
Tom0108 1:199c4a71da88 451 else i2c.Out_Set(Out_Data,4,0);
Tom0108 0:761a63c6d020 452
Tom0108 3:1063aa133b62 453 if(unfold_f)i2c.Out_Set(Out_Data,3,1);
Tom0108 3:1063aa133b62 454 else i2c.Out_Set(Out_Data,3,0);
Tom0108 0:761a63c6d020 455
Tom0108 0:761a63c6d020 456
Tom0108 3:1063aa133b62 457 /* ----------------ボタン系--------------- */
Tom0108 3:1063aa133b62 458 //補正角リセット
Tom0108 2:47954f05d32d 459 if(sbdbt.L1) {
Tom0108 5:fcc79e507610 460 Turn_val=y_deg[0];
Tom0108 5:fcc79e507610 461 offset=y_deg[0];
Tom0108 2:47954f05d32d 462 }
Tom0108 2:47954f05d32d 463 //90度回転して補正の角度を固定する
Tom0108 3:1063aa133b62 464 if(LEFTkey) {
Tom0108 3:1063aa133b62 465 if(once==0) {
Tom0108 4:236e5c58e8ee 466 dir++;
Tom0108 4:236e5c58e8ee 467 // left_f++;
Tom0108 4:236e5c58e8ee 468 // left_f%=2;
Tom0108 3:1063aa133b62 469 once++;
Tom0108 3:1063aa133b62 470 }
Tom0108 3:1063aa133b62 471 } else if(RIGHTkey) {
Tom0108 3:1063aa133b62 472 if(once==0) {
Tom0108 4:236e5c58e8ee 473 dir--;
Tom0108 4:236e5c58e8ee 474 // right_f++;
Tom0108 4:236e5c58e8ee 475 // right_f%=2;
Tom0108 3:1063aa133b62 476 once++;
Tom0108 3:1063aa133b62 477 }
Tom0108 2:47954f05d32d 478 }
Tom0108 3:1063aa133b62 479 //かごを倒す
Tom0108 3:1063aa133b62 480 else if(CROSS) {
Tom0108 1:199c4a71da88 481 if(once==0) {
Tom0108 3:1063aa133b62 482 kago_f++;
Tom0108 3:1063aa133b62 483 kago_f%=2;
Tom0108 1:199c4a71da88 484 once++;
Tom0108 1:199c4a71da88 485 }
Tom0108 3:1063aa133b62 486 }
Tom0108 3:1063aa133b62 487 //タオルを掴んで動かす
Tom0108 3:1063aa133b62 488 else if(SQUARE) {
Tom0108 1:199c4a71da88 489 if(once==0) {
Tom0108 3:1063aa133b62 490 grab_f++;
Tom0108 3:1063aa133b62 491 grab_f%=2;
Tom0108 1:199c4a71da88 492 once++;
Tom0108 1:199c4a71da88 493 }
Tom0108 4:236e5c58e8ee 494 } else if(TRIANGLE) {
Tom0108 4:236e5c58e8ee 495 if(once==0) {
Tom0108 4:236e5c58e8ee 496 sgrab_f++;
Tom0108 4:236e5c58e8ee 497 sgrab_f%=2;
Tom0108 4:236e5c58e8ee 498 once++;
Tom0108 4:236e5c58e8ee 499 }
Tom0108 3:1063aa133b62 500 }
Tom0108 3:1063aa133b62 501 //倒すを引いて動かす
Tom0108 3:1063aa133b62 502 else if(CIRCLE) {
Tom0108 1:199c4a71da88 503 if(once==0) {
Tom0108 3:1063aa133b62 504 pull_f++;
Tom0108 3:1063aa133b62 505 pull_f%=2;
Tom0108 3:1063aa133b62 506 once++;
Tom0108 3:1063aa133b62 507 }
Tom0108 3:1063aa133b62 508 }
Tom0108 3:1063aa133b62 509 //タオル展開用
Tom0108 3:1063aa133b62 510 else if(sbdbt.START) {
Tom0108 3:1063aa133b62 511 if(once==0) {
Tom0108 3:1063aa133b62 512 unfold_f++;
Tom0108 3:1063aa133b62 513 unfold_f%=2;
Tom0108 1:199c4a71da88 514 once++;
Tom0108 1:199c4a71da88 515 }
Tom0108 1:199c4a71da88 516 } else once=0;
Tom0108 1:199c4a71da88 517
Tom0108 1:199c4a71da88 518
Tom0108 0:761a63c6d020 519 } // if(sbdbt.State_check())
Tom0108 0:761a63c6d020 520 } // while(1)
Tom0108 0:761a63c6d020 521 } // int main()
Tom0108 0:761a63c6d020 522
Tom0108 0:761a63c6d020 523 /* メカナムの基本移動 */
Tom0108 0:761a63c6d020 524 void mecanum_Move(void)
Tom0108 0:761a63c6d020 525 {
Tom0108 0:761a63c6d020 526 int val1,val2;
Tom0108 0:761a63c6d020 527
Tom0108 0:761a63c6d020 528 MD_SET_DRIVE(MD_Data, 0,(L > 0) ? MD_FORWARD : MD_REVERSE);
Tom0108 0:761a63c6d020 529 MD_SET_DRIVE(MD_Data, 1,(R > 0) ? MD_REVERSE : MD_FORWARD);
Tom0108 0:761a63c6d020 530 MD_SET_DRIVE(MD_Data, 2,(R > 0) ? MD_FORWARD : MD_REVERSE);
Tom0108 0:761a63c6d020 531 MD_SET_DRIVE(MD_Data, 3,(L > 0) ? MD_REVERSE : MD_FORWARD);
Tom0108 0:761a63c6d020 532
Tom0108 0:761a63c6d020 533 val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly ) );
Tom0108 0:761a63c6d020 534 val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx ) );
Tom0108 0:761a63c6d020 535
Tom0108 0:761a63c6d020 536 MD_SET_PWM(MD_Data,0,val1);
Tom0108 0:761a63c6d020 537 MD_SET_PWM(MD_Data,1,val2);
Tom0108 0:761a63c6d020 538 MD_SET_PWM(MD_Data,2,val2);
Tom0108 0:761a63c6d020 539 MD_SET_PWM(MD_Data,3,val1);
Tom0108 0:761a63c6d020 540
Tom0108 0:761a63c6d020 541
Tom0108 0:761a63c6d020 542 if(MD_GET_PWM(MD_Data,0)*2 < MD_GET_PWM(MD_Data,1) ) {
Tom0108 0:761a63c6d020 543 MD_SET_PWM(MD_Data, 0, 0);
Tom0108 0:761a63c6d020 544 g = 0;
Tom0108 0:761a63c6d020 545 } else if( MD_GET_PWM(MD_Data,1)*2 < MD_GET_PWM(MD_Data,0) ) {
Tom0108 0:761a63c6d020 546 MD_SET_PWM(MD_Data, 1, 0);
Tom0108 0:761a63c6d020 547 g = 0;
Tom0108 0:761a63c6d020 548 }
Tom0108 0:761a63c6d020 549
Tom0108 0:761a63c6d020 550 // 斜めじゃないなら大きいほうにPWMをそろえて完全縦横移動
Tom0108 0:761a63c6d020 551 else if( MD_GET_PWM(MD_Data,0) < MD_GET_PWM(MD_Data,1) ) {
Tom0108 0:761a63c6d020 552 MD_SET_PWM(MD_Data, 0, MD_GET_PWM(MD_Data,1));
Tom0108 0:761a63c6d020 553 g = 1;
Tom0108 0:761a63c6d020 554
Tom0108 0:761a63c6d020 555 } else {
Tom0108 0:761a63c6d020 556 MD_SET_PWM(MD_Data, 1, MD_GET_PWM(MD_Data,0));
Tom0108 0:761a63c6d020 557 g = 1;
Tom0108 0:761a63c6d020 558 }
Tom0108 0:761a63c6d020 559
Tom0108 0:761a63c6d020 560 MD_SET_PWM(MD_Data, 2, MD_GET_PWM(MD_Data,1));
Tom0108 0:761a63c6d020 561 MD_SET_PWM(MD_Data, 3, MD_GET_PWM(MD_Data,0));
Tom0108 0:761a63c6d020 562
Tom0108 0:761a63c6d020 563 }
Tom0108 0:761a63c6d020 564
Tom0108 0:761a63c6d020 565 /* メカナム旋回 */
Tom0108 0:761a63c6d020 566 void mecanum_Turn(void)
Tom0108 0:761a63c6d020 567 {
Tom0108 0:761a63c6d020 568 for(int i=0; i<4; i++) {
Tom0108 0:761a63c6d020 569 MD_SET_PWM(MD_Data, i,sbdbt.R1?abs(sbdbt.RX - 64)*50/64:abs(sbdbt.RX - 64)*100/64);
Tom0108 0:761a63c6d020 570 MD_SET_DRIVE(MD_Data, i,(sbdbt.RX < 64)? MD_REVERSE: MD_FORWARD);
Tom0108 0:761a63c6d020 571 }
Tom0108 0:761a63c6d020 572 }
Tom0108 0:761a63c6d020 573
Tom0108 0:761a63c6d020 574 /* 停止 */
Tom0108 0:761a63c6d020 575 void mecanum_Stop(void)
Tom0108 0:761a63c6d020 576 {
Tom0108 0:761a63c6d020 577 for(int i=0; i<4; i++) {
Tom0108 0:761a63c6d020 578 MD_SET_PWM(MD_Data, i, 0);
Tom0108 0:761a63c6d020 579 MD_SET_DRIVE(MD_Data, i, MD_BRAKE);
Tom0108 0:761a63c6d020 580 }
Tom0108 0:761a63c6d020 581 }
Tom0108 0:761a63c6d020 582
Tom0108 0:761a63c6d020 583 /*操作法*/
Tom0108 0:761a63c6d020 584 /*
Tom0108 0:761a63c6d020 585 〇 ボタン == 回収（引きずる）
Tom0108 0:761a63c6d020 586 ? ボタン == 籠
Tom0108 0:761a63c6d020 587 △ ボタン == 回収（シーツ兼シャツ）
Tom0108 0:761a63c6d020 588 □ ボタン == 回収（タオル）
Tom0108 0:761a63c6d020 589 上 ボタン == ハンガーかけ機構 上昇
Tom0108 0:761a63c6d020 590 下 ボタン == ハンガーかけ機構 下降
Tom0108 0:761a63c6d020 591 左 ボタン == 90回転
Tom0108 0:761a63c6d020 592 右 ボタン == 90回転
Tom0108 0:761a63c6d020 593 L1 ボタン == リセット（足回り）
Tom0108 0:761a63c6d020 594 L2 ボタン == タオル掛け
Tom0108 0:761a63c6d020 595 R1 ボタン == 押しながら回転でゆっくりになる
Tom0108 0:761a63c6d020 596 R2 ボタン == タオルひっぱる奴
Tom0108 0:761a63c6d020 597 select == ハンガー掛け 最大まで上昇
Tom0108 0:761a63c6d020 598 start == ハンガー展開
Tom0108 0:761a63c6d020 599 */
Tom0108 0:761a63c6d020 600 /*その他注意点*/
Tom0108 0:761a63c6d020 601 /*
Tom0108 0:761a63c6d020 602 動かすときはジャイロの値を安定させるため15秒から20秒待つ
Tom0108 0:761a63c6d020 603 メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
Tom0108 0:761a63c6d020 604 あまり急発進をさせない（たまに暴走）
Tom0108 0:761a63c6d020 605 暴走したときはコントローラーを動かしたら治るはず
Tom0108 0:761a63c6d020 606 コントローラー接続が切れたらSTMの黒い奴を押す（わからないときは近くの回路班へ）
Tom0108 0:761a63c6d020 607 */