Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@3:1063aa133b62, 2019-08-23 (annotated)

Committer:
Tom0108
Date:
Fri Aug 23 06:27:19 2019 +0000
Revision:
3:1063aa133b62
Parent:
2:47954f05d32d
Child:
4:236e5c58e8ee
I wrote the program properly

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