Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@0:761a63c6d020, 2019-08-22 (annotated)

Committer:
Tom0108
Date:
Thu Aug 22 09:26:20 2019 +0000
Revision:
0:761a63c6d020
Child:
1:199c4a71da88
08/22

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