Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@1:199c4a71da88, 2019-08-23 (annotated)

Committer:
Tom0108
Date:
Fri Aug 23 02:18:37 2019 +0000
Revision:
1:199c4a71da88
Parent:
0:761a63c6d020
Child:
2:47954f05d32d
jidoukeiugoita

Who changed what in which revision?

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