Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@2:47954f05d32d, 2019-08-23 (annotated)

Committer:
Tom0108
Date:
Fri Aug 23 03:54:05 2019 +0000
Revision:
2:47954f05d32d
Parent:
1:199c4a71da88
Child:
3:1063aa133b62
Basic operation is possible

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 2:47954f05d32d 49 {0,1,0}, // 爪
Tom0108 2:47954f05d32d 50 {0,2,0}, // タオルの先
Tom0108 2:47954f05d32d 51 {0,3,0}, // タオル展開
Tom0108 2:47954f05d32d 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 yaw = -y_deg+offset;
Tom0108 0:761a63c6d020 155 dif_val=y_deg-Turn_val;
Tom0108 0:761a63c6d020 156
Tom0108 0:761a63c6d020 157 if(sbdbt.LX !=64 || sbdbt.LY !=64) {
Tom0108 0:761a63c6d020 158
Tom0108 0:761a63c6d020 159 mecanum_Move();
Tom0108 0:761a63c6d020 160
Tom0108 0:761a63c6d020 161 // 以下，ジャイロセンサーのプログラム
Tom0108 0:761a63c6d020 162 if(g) {
Tom0108 0:761a63c6d020 163 if(abs(ly) > 10) {
Tom0108 0:761a63c6d020 164 if(ly>10) {
Tom0108 0:761a63c6d020 165 (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
Tom0108 0:761a63c6d020 166 (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)+dif_val;
Tom0108 0:761a63c6d020 167 } else {
Tom0108 0:761a63c6d020 168 (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
Tom0108 0:761a63c6d020 169 (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)-dif_val;
Tom0108 0:761a63c6d020 170 }
Tom0108 0:761a63c6d020 171 }
Tom0108 0:761a63c6d020 172
Tom0108 0:761a63c6d020 173 if(abs(lx) > 10) {
Tom0108 0:761a63c6d020 174 if(lx>10) {
Tom0108 0:761a63c6d020 175 (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
Tom0108 0:761a63c6d020 176 (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)+dif_val;
Tom0108 0:761a63c6d020 177 } else {
Tom0108 0:761a63c6d020 178 (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
Tom0108 0:761a63c6d020 179 (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)-dif_val;
Tom0108 0:761a63c6d020 180 }
Tom0108 0:761a63c6d020 181 }
Tom0108 0:761a63c6d020 182
Tom0108 0:761a63c6d020 183 if((abs(ly)-abs(lx)) > 0) {
Tom0108 0:761a63c6d020 184 (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,0);
Tom0108 0:761a63c6d020 185 (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,1);
Tom0108 0:761a63c6d020 186 } else {
Tom0108 0:761a63c6d020 187 (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,0);
Tom0108 0:761a63c6d020 188 (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,2);
Tom0108 0:761a63c6d020 189 }
Tom0108 0:761a63c6d020 190 } // if(g)
Tom0108 0:761a63c6d020 191 }// if(LX !=64 || LY !=64)
Tom0108 0:761a63c6d020 192 else if(sbdbt.RX != 64) { // 旋回
Tom0108 0:761a63c6d020 193 Turn_val=y_deg;
Tom0108 0:761a63c6d020 194 mecanum_Turn();
Tom0108 0:761a63c6d020 195 } else {
Tom0108 0:761a63c6d020 196 mecanum_Stop();
Tom0108 0:761a63c6d020 197 }
Tom0108 0:761a63c6d020 198 if(left_f) { // 90°旋回
Tom0108 0:761a63c6d020 199 if(abs(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_REVERSE);
Tom0108 0:761a63c6d020 203 }
Tom0108 0:761a63c6d020 204 } else {
Tom0108 0:761a63c6d020 205 left_f=0;
Tom0108 0:761a63c6d020 206 mecanum_Stop();
Tom0108 0:761a63c6d020 207 }
Tom0108 0:761a63c6d020 208 }
Tom0108 0:761a63c6d020 209 if(right_f) {
Tom0108 0:761a63c6d020 210 if(yaw>-60) {
Tom0108 0:761a63c6d020 211 for(int i=0; i<4; i++) {
Tom0108 0:761a63c6d020 212 MD_SET_PWM(MD_Data, i,60);
Tom0108 0:761a63c6d020 213 MD_SET_DRIVE(MD_Data, i,MD_FORWARD);
Tom0108 0:761a63c6d020 214 }
Tom0108 0:761a63c6d020 215 } else {
Tom0108 0:761a63c6d020 216 right_f=0;
Tom0108 0:761a63c6d020 217 mecanum_Stop();
Tom0108 0:761a63c6d020 218 }
Tom0108 0:761a63c6d020 219 }
Tom0108 0:761a63c6d020 220
Tom0108 0:761a63c6d020 221 /*---ハンガーかけるやつ--------------------------------------------------------------------------------------------------------------------------------------------*/
Tom0108 0:761a63c6d020 222 limit_up = (In_Data2+0)->in_data;
Tom0108 0:761a63c6d020 223 limit_down = (In_Data2+1)->in_data;
Tom0108 1:199c4a71da88 224 if(sbdbt.SELECT)as=1;//selectキー
Tom0108 0:761a63c6d020 225
Tom0108 0:761a63c6d020 226 if(as==1 && limit_up == 0) {
Tom0108 0:761a63c6d020 227 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 0:761a63c6d020 228 MD_SET_PWM(MD_Data, 5,50);
Tom0108 0:761a63c6d020 229 } else if(UPkey && limit_up == 0) {
Tom0108 0:761a63c6d020 230 as=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(as==1 && limit_up == 1) {
Tom0108 0:761a63c6d020 234 as=0;
Tom0108 0:761a63c6d020 235 MD_SET_PWM(MD_Data,5,0);
Tom0108 0:761a63c6d020 236 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 0:761a63c6d020 237 } else if(DOWNkey && limit_down == 0) {
Tom0108 0:761a63c6d020 238 MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
Tom0108 0:761a63c6d020 239 MD_SET_PWM(MD_Data, 5,50);
Tom0108 0:761a63c6d020 240 } else {
Tom0108 0:761a63c6d020 241 MD_SET_PWM(MD_Data,5,0);
Tom0108 0:761a63c6d020 242 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 0:761a63c6d020 243 }
Tom0108 0:761a63c6d020 244
Tom0108 0:761a63c6d020 245 max_lim = (In_Data+1)->in_data;
Tom0108 0:761a63c6d020 246 start_lim =(In_Data+2)->in_data;
Tom0108 1:199c4a71da88 247 short_lim = (In_Data2+2)->in_data;
Tom0108 2:47954f05d32d 248
Tom0108 1:199c4a71da88 249 //タオルつかむ 四角
Tom0108 1:199c4a71da88 250 if(grab_flag) {
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 1:199c4a71da88 257 MD_SET_PWM (MD_Data,4,40);
Tom0108 1:199c4a71da88 258 if(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 2:47954f05d32d 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 1:199c4a71da88 271 if(tim.read_ms()>1000) {
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 1:199c4a71da88 280 if(tim.read_ms()>500) {
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 1:199c4a71da88 289 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 1:199c4a71da88 290 MD_SET_PWM (MD_Data,4,40);
Tom0108 1:199c4a71da88 291 if(start_lim==1) grab_flag=0;
Tom0108 1:199c4a71da88 292 break;
Tom0108 0:761a63c6d020 293 }
Tom0108 1:199c4a71da88 294 } //if(grab_flag)
Tom0108 2:47954f05d32d 295 //タオル引く 丸
Tom0108 1:199c4a71da88 296 else if(pull_flag) {
Tom0108 1:199c4a71da88 297 switch(pull) {
Tom0108 1:199c4a71da88 298 //腕を前に動かす
Tom0108 1:199c4a71da88 299 case 0:
Tom0108 1:199c4a71da88 300 //爪閉じる
Tom0108 1:199c4a71da88 301 i2c.Out_Set(Out_Data,1,0);
Tom0108 0:761a63c6d020 302 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 1:199c4a71da88 303 MD_SET_PWM (MD_Data,4,40);
Tom0108 1:199c4a71da88 304 if(max_lim==1) {
Tom0108 1:199c4a71da88 305 pull++;
Tom0108 1:199c4a71da88 306 tim.reset();
Tom0108 1:199c4a71da88 307 }
Tom0108 1:199c4a71da88 308 break;
Tom0108 0:761a63c6d020 309
Tom0108 1:199c4a71da88 310 case 1:
Tom0108 2:47954f05d32d 311 //腕停止
Tom0108 2:47954f05d32d 312 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 2:47954f05d32d 313 MD_SET_PWM (MD_Data,4,0);
Tom0108 2:47954f05d32d 314
Tom0108 1:199c4a71da88 315 //腕おろす
Tom0108 0:761a63c6d020 316 i2c.Out_Set(Out_Data,0,1);
Tom0108 1:199c4a71da88 317 if(tim.read_ms()>1000) {
Tom0108 1:199c4a71da88 318 pull++;
Tom0108 1:199c4a71da88 319 tim.reset();
Tom0108 0:761a63c6d020 320 }
Tom0108 1:199c4a71da88 321 break;
Tom0108 1:199c4a71da88 322
Tom0108 1:199c4a71da88 323 case 2:
Tom0108 1:199c4a71da88 324 if(tim.read_ms()>500) {
Tom0108 1:199c4a71da88 325 pull++;
Tom0108 0:761a63c6d020 326 tim.reset();
Tom0108 0:761a63c6d020 327 }
Tom0108 1:199c4a71da88 328 break;
Tom0108 1:199c4a71da88 329
Tom0108 1:199c4a71da88 330 case 3:
Tom0108 1:199c4a71da88 331 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 1:199c4a71da88 332 MD_SET_PWM (MD_Data,4,40);
Tom0108 1:199c4a71da88 333 if(start_lim==1) pull_flag=0;
Tom0108 1:199c4a71da88 334 break;
Tom0108 0:761a63c6d020 335 }
Tom0108 1:199c4a71da88 336 } //if(pull_flag)
Tom0108 1:199c4a71da88 337 else if(kago_flag) {
Tom0108 1:199c4a71da88 338 //腕の先を閉じておく
Tom0108 1:199c4a71da88 339 i2c.Out_Set(Out_Data,1,0);
Tom0108 0:761a63c6d020 340
Tom0108 1:199c4a71da88 341 switch(kago) {
Tom0108 1:199c4a71da88 342 //腕を前に動かす
Tom0108 1:199c4a71da88 343 case 0:
Tom0108 0:761a63c6d020 344 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 1:199c4a71da88 345 MD_SET_PWM (MD_Data,4,40);
Tom0108 2:47954f05d32d 346 if(max_lim==1) {
Tom0108 2:47954f05d32d 347 kago++;
Tom0108 2:47954f05d32d 348 tim.reset();
Tom0108 2:47954f05d32d 349 }
Tom0108 1:199c4a71da88 350 break;
Tom0108 1:199c4a71da88 351
Tom0108 1:199c4a71da88 352 //かごを倒す
Tom0108 1:199c4a71da88 353 case 1:
Tom0108 1:199c4a71da88 354 //腕停止
Tom0108 0:761a63c6d020 355 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 1:199c4a71da88 356 MD_SET_PWM (MD_Data,4,0);
Tom0108 0:761a63c6d020 357
Tom0108 1:199c4a71da88 358 //かご回転
Tom0108 1:199c4a71da88 359 MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
Tom0108 1:199c4a71da88 360 MD_SET_PWM (MD_Data,6,70);
Tom0108 2:47954f05d32d 361 if(slide_stop==1 || tim.read_ms()>3000) {
Tom0108 1:199c4a71da88 362 kago++;
Tom0108 2:47954f05d32d 363 tim.reset();
Tom0108 1:199c4a71da88 364 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 1:199c4a71da88 365 MD_SET_PWM (MD_Data,6,0);
Tom0108 1:199c4a71da88 366 }
Tom0108 1:199c4a71da88 367 break;
Tom0108 0:761a63c6d020 368
Tom0108 1:199c4a71da88 369 case 2:
Tom0108 2:47954f05d32d 370 if(tim.read_ms()>1500) {
Tom0108 2:47954f05d32d 371 kago++;
Tom0108 2:47954f05d32d 372 tim.reset();
Tom0108 2:47954f05d32d 373 }
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 2:47954f05d32d 379 if(slide_start==1 || tim.read_ms()>3000) 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 2:47954f05d32d 395 i2c.Out_Set(Out_Data,1,1);
Tom0108 1:199c4a71da88 396 //腕上げる
Tom0108 2:47954f05d32d 397 i2c.Out_Set(Out_Data,0,0);
Tom0108 2:47954f05d32d 398
Tom0108 1:199c4a71da88 399 kago=0;
Tom0108 1:199c4a71da88 400 pull=0;
Tom0108 1:199c4a71da88 401 grab=0;
Tom0108 2:47954f05d32d 402
Tom0108 2:47954f05d32d 403 tim.reset();
Tom0108 0:761a63c6d020 404 }
Tom0108 0:761a63c6d020 405
Tom0108 0:761a63c6d020 406 /*タオル掛け*/
Tom0108 0:761a63c6d020 407 if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
Tom0108 0:761a63c6d020 408 else i2c.Out_Set(Out_Data,2,0);
Tom0108 0:761a63c6d020 409 /*タオルを引っ張るやつ*/
Tom0108 1:199c4a71da88 410 if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
Tom0108 1:199c4a71da88 411 else i2c.Out_Set(Out_Data,4,0);
Tom0108 0:761a63c6d020 412 /*展開*/
Tom0108 1:199c4a71da88 413 if(sbdbt.START && start_f == 0) {
Tom0108 0:761a63c6d020 414 start_cnt++;
Tom0108 0:761a63c6d020 415 start_f = 1;
Tom0108 1:199c4a71da88 416 } else if(sbdbt.START==0 && start_f == 1) start_f =0;
Tom0108 0:761a63c6d020 417
Tom0108 0:761a63c6d020 418 if(start_cnt%3==1) start_s=0;
Tom0108 0:761a63c6d020 419 else start_s=1;
Tom0108 0:761a63c6d020 420
Tom0108 1:199c4a71da88 421 if(start_s==0)i2c.Out_Set(Out_Data,3,1);
Tom0108 1:199c4a71da88 422 else if(start_s==1)i2c.Out_Set(Out_Data,3,0);
Tom0108 1:199c4a71da88 423 else start_cnt=2;
Tom0108 0:761a63c6d020 424 //-------------- 出力 --------------//
Tom0108 0:761a63c6d020 425 i2c.Out(Out_Data,5);
Tom0108 0:761a63c6d020 426
Tom0108 0:761a63c6d020 427 i2c.MD_I2C(MD_Data,0);
Tom0108 0:761a63c6d020 428 i2c.MD_I2C(MD_Data,1);
Tom0108 0:761a63c6d020 429 i2c.MD_I2C(MD_Data,2);
Tom0108 0:761a63c6d020 430 i2c.MD_I2C(MD_Data,3);
Tom0108 0:761a63c6d020 431 i2c.MD_I2C(MD_Data,4);
Tom0108 0:761a63c6d020 432 i2c.MD_I2C(MD_Data,5);
Tom0108 0:761a63c6d020 433 i2c.MD_I2C(MD_Data,6);
Tom0108 0:761a63c6d020 434 i2c.MD_I2C(MD_Data,7);
Tom0108 0:761a63c6d020 435
Tom0108 1:199c4a71da88 436 pc.printf("pull %2d%2d ",pull,pull_flag);
Tom0108 1:199c4a71da88 437 pc.printf("grab %2d%2d ",grab,grab_flag);
Tom0108 1:199c4a71da88 438 pc.printf("kago %2d%2d ",kago,kago_flag);
Tom0108 1:199c4a71da88 439 pc.printf("\n\r");
Tom0108 1:199c4a71da88 440 // pc.printf("%2d%2d%2d%2d%2d%2d\n\r",UPkey,RIGHTkey,DOWNkey,LEFTkey,sbdbt.SELECT,sbdbt.START);
Tom0108 0:761a63c6d020 441 // 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 442
Tom0108 2:47954f05d32d 443 //ボタン系
Tom0108 2:47954f05d32d 444 if(sbdbt.L1) {
Tom0108 2:47954f05d32d 445 Turn_val=y_deg;
Tom0108 2:47954f05d32d 446 offset=y_deg;
Tom0108 2:47954f05d32d 447 }
Tom0108 2:47954f05d32d 448
Tom0108 2:47954f05d32d 449 //90度回転して補正の角度を固定する
Tom0108 2:47954f05d32d 450 if(LEFTkey) offset=y_deg;
Tom0108 2:47954f05d32d 451 if(RIGHTkey) {
Tom0108 2:47954f05d32d 452 offset=y_deg;
Tom0108 2:47954f05d32d 453 right_f=1;
Tom0108 2:47954f05d32d 454 }
Tom0108 1:199c4a71da88 455 if(CROSS) {
Tom0108 1:199c4a71da88 456 if(once==0) {
Tom0108 1:199c4a71da88 457 kago_flag++;
Tom0108 1:199c4a71da88 458 kago_flag%=2;
Tom0108 1:199c4a71da88 459 once++;
Tom0108 1:199c4a71da88 460 }
Tom0108 1:199c4a71da88 461 } else if(SQUARE) {
Tom0108 1:199c4a71da88 462 if(once==0) {
Tom0108 1:199c4a71da88 463 grab_flag++;
Tom0108 1:199c4a71da88 464 grab_flag%=2;
Tom0108 1:199c4a71da88 465 once++;
Tom0108 1:199c4a71da88 466 }
Tom0108 1:199c4a71da88 467 } else if(CIRCLE) {
Tom0108 1:199c4a71da88 468 if(once==0) {
Tom0108 1:199c4a71da88 469 pull_flag++;
Tom0108 1:199c4a71da88 470 pull_flag%=2;
Tom0108 1:199c4a71da88 471 once++;
Tom0108 1:199c4a71da88 472 }
Tom0108 1:199c4a71da88 473 } else once=0;
Tom0108 1:199c4a71da88 474
Tom0108 1:199c4a71da88 475
Tom0108 0:761a63c6d020 476 } // if(sbdbt.State_check())
Tom0108 0:761a63c6d020 477 } // while(1)
Tom0108 0:761a63c6d020 478 } // int main()
Tom0108 0:761a63c6d020 479
Tom0108 0:761a63c6d020 480 /* メカナムの基本移動 */
Tom0108 0:761a63c6d020 481 void mecanum_Move(void)
Tom0108 0:761a63c6d020 482 {
Tom0108 0:761a63c6d020 483 int val1,val2;
Tom0108 0:761a63c6d020 484
Tom0108 0:761a63c6d020 485 MD_SET_DRIVE(MD_Data, 0,(L > 0) ? MD_FORWARD : MD_REVERSE);
Tom0108 0:761a63c6d020 486 MD_SET_DRIVE(MD_Data, 1,(R > 0) ? MD_REVERSE : MD_FORWARD);
Tom0108 0:761a63c6d020 487 MD_SET_DRIVE(MD_Data, 2,(R > 0) ? MD_FORWARD : MD_REVERSE);
Tom0108 0:761a63c6d020 488 MD_SET_DRIVE(MD_Data, 3,(L > 0) ? MD_REVERSE : MD_FORWARD);
Tom0108 0:761a63c6d020 489
Tom0108 0:761a63c6d020 490 val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly ) );
Tom0108 0:761a63c6d020 491 val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx ) );
Tom0108 0:761a63c6d020 492
Tom0108 0:761a63c6d020 493 MD_SET_PWM(MD_Data,0,val1);
Tom0108 0:761a63c6d020 494 MD_SET_PWM(MD_Data,1,val2);
Tom0108 0:761a63c6d020 495 MD_SET_PWM(MD_Data,2,val2);
Tom0108 0:761a63c6d020 496 MD_SET_PWM(MD_Data,3,val1);
Tom0108 0:761a63c6d020 497
Tom0108 0:761a63c6d020 498
Tom0108 0:761a63c6d020 499 if(MD_GET_PWM(MD_Data,0)*2 < MD_GET_PWM(MD_Data,1) ) {
Tom0108 0:761a63c6d020 500 MD_SET_PWM(MD_Data, 0, 0);
Tom0108 0:761a63c6d020 501 g = 0;
Tom0108 0:761a63c6d020 502 } else if( MD_GET_PWM(MD_Data,1)*2 < MD_GET_PWM(MD_Data,0) ) {
Tom0108 0:761a63c6d020 503 MD_SET_PWM(MD_Data, 1, 0);
Tom0108 0:761a63c6d020 504 g = 0;
Tom0108 0:761a63c6d020 505 }
Tom0108 0:761a63c6d020 506
Tom0108 0:761a63c6d020 507 // 斜めじゃないなら大きいほうにPWMをそろえて完全縦横移動
Tom0108 0:761a63c6d020 508 else if( MD_GET_PWM(MD_Data,0) < MD_GET_PWM(MD_Data,1) ) {
Tom0108 0:761a63c6d020 509 MD_SET_PWM(MD_Data, 0, MD_GET_PWM(MD_Data,1));
Tom0108 0:761a63c6d020 510 g = 1;
Tom0108 0:761a63c6d020 511
Tom0108 0:761a63c6d020 512 } else {
Tom0108 0:761a63c6d020 513 MD_SET_PWM(MD_Data, 1, MD_GET_PWM(MD_Data,0));
Tom0108 0:761a63c6d020 514 g = 1;
Tom0108 0:761a63c6d020 515 }
Tom0108 0:761a63c6d020 516
Tom0108 0:761a63c6d020 517 MD_SET_PWM(MD_Data, 2, MD_GET_PWM(MD_Data,1));
Tom0108 0:761a63c6d020 518 MD_SET_PWM(MD_Data, 3, MD_GET_PWM(MD_Data,0));
Tom0108 0:761a63c6d020 519
Tom0108 0:761a63c6d020 520 }
Tom0108 0:761a63c6d020 521
Tom0108 0:761a63c6d020 522 /* メカナム旋回 */
Tom0108 0:761a63c6d020 523 void mecanum_Turn(void)
Tom0108 0:761a63c6d020 524 {
Tom0108 0:761a63c6d020 525 for(int i=0; i<4; i++) {
Tom0108 0:761a63c6d020 526 MD_SET_PWM(MD_Data, i,sbdbt.R1?abs(sbdbt.RX - 64)*50/64:abs(sbdbt.RX - 64)*100/64);
Tom0108 0:761a63c6d020 527 MD_SET_DRIVE(MD_Data, i,(sbdbt.RX < 64)? MD_REVERSE: MD_FORWARD);
Tom0108 0:761a63c6d020 528 }
Tom0108 0:761a63c6d020 529 }
Tom0108 0:761a63c6d020 530
Tom0108 0:761a63c6d020 531 /*メカナム旋廻移動*/
Tom0108 0:761a63c6d020 532 void mecanum_TurnMove(void)
Tom0108 0:761a63c6d020 533 {
Tom0108 0:761a63c6d020 534 int val1,val2;
Tom0108 0:761a63c6d020 535
Tom0108 0:761a63c6d020 536 MD_SET_DRIVE(MD_Data, 0,(L_1 > 0) ? MD_FORWARD : MD_REVERSE);
Tom0108 0:761a63c6d020 537 MD_SET_DRIVE(MD_Data, 1,(R_1 > 0) ? MD_REVERSE : MD_FORWARD);
Tom0108 0:761a63c6d020 538 MD_SET_DRIVE(MD_Data, 2,(R_1 > 0) ? MD_FORWARD : MD_REVERSE);
Tom0108 0:761a63c6d020 539 MD_SET_DRIVE(MD_Data, 3,(L_1 > 0) ? MD_REVERSE : MD_FORWARD);
Tom0108 0:761a63c6d020 540
Tom0108 0:761a63c6d020 541 val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly +rx));
Tom0108 0:761a63c6d020 542 val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx +rx));
Tom0108 0:761a63c6d020 543
Tom0108 0:761a63c6d020 544 MD_SET_PWM(MD_Data,0,val1);
Tom0108 0:761a63c6d020 545 MD_SET_PWM(MD_Data,1,val2);
Tom0108 0:761a63c6d020 546 MD_SET_PWM(MD_Data,2,val2);
Tom0108 0:761a63c6d020 547 MD_SET_PWM(MD_Data,3,val1);
Tom0108 0:761a63c6d020 548 }
Tom0108 0:761a63c6d020 549 /* 停止 */
Tom0108 0:761a63c6d020 550 void mecanum_Stop(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, 0);
Tom0108 0:761a63c6d020 554 MD_SET_DRIVE(MD_Data, i, MD_BRAKE);
Tom0108 0:761a63c6d020 555 }
Tom0108 0:761a63c6d020 556 }
Tom0108 0:761a63c6d020 557
Tom0108 0:761a63c6d020 558
Tom0108 0:761a63c6d020 559 /*割り込み*/
Tom0108 0:761a63c6d020 560 // void flip1(void) {
Tom0108 0:761a63c6d020 561 // static int cnt=0;
Tom0108 0:761a63c6d020 562 //
Tom0108 0:761a63c6d020 563 // /* カウントアップ，1sループ */
Tom0108 0:761a63c6d020 564 // cnt++;
Tom0108 0:761a63c6d020 565 // cnt%=10000;
Tom0108 0:761a63c6d020 566 //
Tom0108 0:761a63c6d020 567 // /* 100msの処理 */
Tom0108 0:761a63c6d020 568 // if(cnt%100==0) {
Tom0108 0:761a63c6d020 569 // myled = !myled;
Tom0108 0:761a63c6d020 570 // if(tim_start==1) time_cnt++;
Tom0108 0:761a63c6d020 571 // if(time_cnt>10 && time_f==0) {
Tom0108 0:761a63c6d020 572 // time_f=1;
Tom0108 0:761a63c6d020 573 // }
Tom0108 0:761a63c6d020 574 // }
Tom0108 0:761a63c6d020 575 //}
Tom0108 0:761a63c6d020 576
Tom0108 0:761a63c6d020 577
Tom0108 0:761a63c6d020 578 /*操作法*/
Tom0108 0:761a63c6d020 579 /*
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 左 ボタン == 90回転
Tom0108 0:761a63c6d020 587 右 ボタン == 90回転
Tom0108 0:761a63c6d020 588 L1 ボタン == リセット（足回り）
Tom0108 0:761a63c6d020 589 L2 ボタン == タオル掛け
Tom0108 0:761a63c6d020 590 R1 ボタン == 押しながら回転でゆっくりになる
Tom0108 0:761a63c6d020 591 R2 ボタン == タオルひっぱる奴
Tom0108 0:761a63c6d020 592 select == ハンガー掛け 最大まで上昇
Tom0108 0:761a63c6d020 593 start == ハンガー展開
Tom0108 0:761a63c6d020 594 */
Tom0108 0:761a63c6d020 595 /*その他注意点*/
Tom0108 0:761a63c6d020 596 /*
Tom0108 0:761a63c6d020 597 動かすときはジャイロの値を安定させるため15秒から20秒待つ
Tom0108 0:761a63c6d020 598 メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
Tom0108 0:761a63c6d020 599 あまり急発進をさせない（たまに暴走）
Tom0108 0:761a63c6d020 600 暴走したときはコントローラーを動かしたら治るはず
Tom0108 0:761a63c6d020 601 コントローラー接続が切れたらSTMの黒い奴を押す（わからないときは近くの回路班へ）
Tom0108 0:761a63c6d020 602 */