Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@4:236e5c58e8ee, 2019-08-23 (annotated)

Committer:
Tom0108
Date:
Fri Aug 23 09:20:04 2019 +0000
Revision:
4:236e5c58e8ee
Parent:
3:1063aa133b62
Child:
5:fcc79e507610
Gyro erase

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