淳熙 姜 / Mbed 2 deprecated mbed_Ahan_robocon

移動用モータ、リフト上下用モータ、シリンダー開閉＆でっぱり用モータの３種類の制御から成り立ったプログラム

Dependencies:   SoftPWM mbed

main.cpp@0:5486cc47e272, 2015-06-24 (annotated)

Committer:
Kansuni
Date:
Wed Jun 24 09:48:01 2015 +0000
Revision:
0:5486cc47e272
?????????????????for ?????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kansuni 0:5486cc47e272 1 //*****************************************
Kansuni 0:5486cc47e272 2 // 部内ロボコン（A班）用コントロールプログラム
Kansuni 0:5486cc47e272 3 //*****************************************
Kansuni 0:5486cc47e272 4
Kansuni 0:5486cc47e272 5 #include "mbed.h"
Kansuni 0:5486cc47e272 6 #include "SoftPWM.h"
Kansuni 0:5486cc47e272 7 #include "main.h"
Kansuni 0:5486cc47e272 8
Kansuni 0:5486cc47e272 9 //デジタル読み取りピン（タクトスイッチ）の指定
Kansuni 0:5486cc47e272 10 DigitalIn Abutton(dp10);
Kansuni 0:5486cc47e272 11 DigitalIn Bbutton(dp14);
Kansuni 0:5486cc47e272 12 DigitalIn Xbutton(dp28);
Kansuni 0:5486cc47e272 13
Kansuni 0:5486cc47e272 14 //デジタル出力ピン（モーター（リフト部分））の指定
Kansuni 0:5486cc47e272 15 DigitalOut motor_Rift1(dp1);
Kansuni 0:5486cc47e272 16 DigitalOut motor_Rift2(dp2);
Kansuni 0:5486cc47e272 17
Kansuni 0:5486cc47e272 18 //デジタル出力ピン（モーター（コップ止め））の指定
Kansuni 0:5486cc47e272 19 DigitalOut motor_Stopper1(dp6);
Kansuni 0:5486cc47e272 20 DigitalOut motor_Stopper2(dp9);
Kansuni 0:5486cc47e272 21
Kansuni 0:5486cc47e272 22 //デジタル出力ピン（空気圧シリンダー）の指定
Kansuni 0:5486cc47e272 23 DigitalOut valve(dp11);
Kansuni 0:5486cc47e272 24
Kansuni 0:5486cc47e272 25 //アナログ読み取りピン（ジョイスティック）の指定
Kansuni 0:5486cc47e272 26 AnalogIn JOYstick_LR(dp13);
Kansuni 0:5486cc47e272 27 AnalogIn JOYstick_UD(dp4);
Kansuni 0:5486cc47e272 28
Kansuni 0:5486cc47e272 29 //PWM出力ピン（モーター）の指定
Kansuni 0:5486cc47e272 30 SoftPWM motor_LEFT1(dp17);
Kansuni 0:5486cc47e272 31 SoftPWM motor_LEFT2(dp18);
Kansuni 0:5486cc47e272 32 SoftPWM motor_RIGHT1(dp25);
Kansuni 0:5486cc47e272 33 SoftPWM motor_RIGHT2(dp26);
Kansuni 0:5486cc47e272 34
Kansuni 0:5486cc47e272 35 //シリアル入出力ピン（デバック用）の指定
Kansuni 0:5486cc47e272 36 Serial Debug(dp16, dp15);
Kansuni 0:5486cc47e272 37
Kansuni 0:5486cc47e272 38 int Cylinder_toggle = 0;
Kansuni 0:5486cc47e272 39 int Cylinder_Open_Flag = 0;
Kansuni 0:5486cc47e272 40 int Cylinder_Close_Flag = 0;
Kansuni 0:5486cc47e272 41 int Cylinder_Open_Standby = 1;
Kansuni 0:5486cc47e272 42 int Cylinder_Close_Standby = 0;
Kansuni 0:5486cc47e272 43 float move_FB = 0.5;
Kansuni 0:5486cc47e272 44 float move_LR = 0.5;
Kansuni 0:5486cc47e272 45
Kansuni 0:5486cc47e272 46 int main(void){
Kansuni 0:5486cc47e272 47
Kansuni 0:5486cc47e272 48 //パルス周期の設定
Kansuni 0:5486cc47e272 49 motor_LEFT1.period_ms(1);
Kansuni 0:5486cc47e272 50 motor_LEFT2.period_ms(1);
Kansuni 0:5486cc47e272 51 motor_RIGHT1.period_ms(1);
Kansuni 0:5486cc47e272 52 motor_RIGHT2.period_ms(1);
Kansuni 0:5486cc47e272 53
Kansuni 0:5486cc47e272 54 //初期設定
Kansuni 0:5486cc47e272 55 motor_Rift1 = 1;
Kansuni 0:5486cc47e272 56 motor_Rift2 = 1;
Kansuni 0:5486cc47e272 57 motor_Stopper1 = 1;
Kansuni 0:5486cc47e272 58 motor_Stopper2 = 1;
Kansuni 0:5486cc47e272 59 valve = 0;
Kansuni 0:5486cc47e272 60 motor_LEFT1 = 0.5;
Kansuni 0:5486cc47e272 61 motor_LEFT2 = 0.5;
Kansuni 0:5486cc47e272 62 motor_RIGHT1 = 0.5;
Kansuni 0:5486cc47e272 63 motor_RIGHT2 = 0.5;
Kansuni 0:5486cc47e272 64
Kansuni 0:5486cc47e272 65 //出力レートの指定
Kansuni 0:5486cc47e272 66 //Debug.baud(9600);
Kansuni 0:5486cc47e272 67
Kansuni 0:5486cc47e272 68 //デバッグ用無限ループ
Kansuni 0:5486cc47e272 69 //Debug_roop();
Kansuni 0:5486cc47e272 70
Kansuni 0:5486cc47e272 71 //Debug.printf("LEFT1 LEFT2 RIGHT1 RIGHT2 \n");
Kansuni 0:5486cc47e272 72
Kansuni 0:5486cc47e272 73 while(1){
Kansuni 0:5486cc47e272 74
Kansuni 0:5486cc47e272 75 move_LR = 1-JOYstick_LR.read();
Kansuni 0:5486cc47e272 76 move_FB = 1-JOYstick_UD.read();
Kansuni 0:5486cc47e272 77
Kansuni 0:5486cc47e272 78 //車体の前後左右のコントロール
Kansuni 0:5486cc47e272 79 Moving_control(move_LR, move_FB);
Kansuni 0:5486cc47e272 80 //リフトの上下操作のコントロール
Kansuni 0:5486cc47e272 81 Rifting_control();
Kansuni 0:5486cc47e272 82 //シリンダーの開閉操作のコントロール
Kansuni 0:5486cc47e272 83 //Cylinder_control();
Kansuni 0:5486cc47e272 84
Kansuni 0:5486cc47e272 85 //フラグ管理
Kansuni 0:5486cc47e272 86 if(Cylinder_Open_Flag >= 1){
Kansuni 0:5486cc47e272 87 Cylinder_Open_Flag = Cylinder_Open_Flag + 1;
Kansuni 0:5486cc47e272 88 }
Kansuni 0:5486cc47e272 89 if(Cylinder_Open_Flag > Iteration){
Kansuni 0:5486cc47e272 90 Cylinder_Open_Flag = 0;
Kansuni 0:5486cc47e272 91 Cylinder_Open_Standby = 1;
Kansuni 0:5486cc47e272 92 //Debug.printf("Cylinder Open Standby \n");
Kansuni 0:5486cc47e272 93 }
Kansuni 0:5486cc47e272 94 if(Cylinder_Close_Flag >= 1){
Kansuni 0:5486cc47e272 95 Cylinder_Close_Flag = Cylinder_Close_Flag + 1;
Kansuni 0:5486cc47e272 96 }
Kansuni 0:5486cc47e272 97 if(Cylinder_Close_Flag > Iteration){
Kansuni 0:5486cc47e272 98 Cylinder_Close_Flag = 0;
Kansuni 0:5486cc47e272 99 Cylinder_Close_Standby = 1;
Kansuni 0:5486cc47e272 100 //Debug.printf("Cylinder Close Standby \n");
Kansuni 0:5486cc47e272 101 }
Kansuni 0:5486cc47e272 102
Kansuni 0:5486cc47e272 103 wait(0.05);
Kansuni 0:5486cc47e272 104
Kansuni 0:5486cc47e272 105 }
Kansuni 0:5486cc47e272 106
Kansuni 0:5486cc47e272 107 }
Kansuni 0:5486cc47e272 108
Kansuni 0:5486cc47e272 109 //void Debug_roop(){
Kansuni 0:5486cc47e272 110 // int l=0;
Kansuni 0:5486cc47e272 111 //
Kansuni 0:5486cc47e272 112 // //なにかPCでシリアル通信で送るまでは動作しない
Kansuni 0:5486cc47e272 113 // while(1){
Kansuni 0:5486cc47e272 114 // wait(0.5);
Kansuni 0:5486cc47e272 115 // if(Debug.readable()){
Kansuni 0:5486cc47e272 116 // break;
Kansuni 0:5486cc47e272 117 // }
Kansuni 0:5486cc47e272 118 // l ^= 1;
Kansuni 0:5486cc47e272 119 // }
Kansuni 0:5486cc47e272 120 //}
Kansuni 0:5486cc47e272 121
Kansuni 0:5486cc47e272 122 //車体の前後左右移動のコントロール
Kansuni 0:5486cc47e272 123 void Moving_control(float x, float y){
Kansuni 0:5486cc47e272 124
Kansuni 0:5486cc47e272 125 /*デバッグ用
Kansuni 0:5486cc47e272 126 Debug.printf("%f ", LEFT1_pow);
Kansuni 0:5486cc47e272 127 Debug.printf("%f ", LEFT2_pow);
Kansuni 0:5486cc47e272 128 Debug.printf("%f ", RIGHT1_pow);
Kansuni 0:5486cc47e272 129 Debug.printf("%f \n", RIGHT2_pow);
Kansuni 0:5486cc47e272 130 */
Kansuni 0:5486cc47e272 131
Kansuni 0:5486cc47e272 132 float x1 = 0;
Kansuni 0:5486cc47e272 133 float x_left = 0;
Kansuni 0:5486cc47e272 134 float x_right = 0;
Kansuni 0:5486cc47e272 135 float y1 = 0;
Kansuni 0:5486cc47e272 136 float V_amount = 0;
Kansuni 0:5486cc47e272 137 float LEFT1_pow = 0.5;
Kansuni 0:5486cc47e272 138 float LEFT2_pow = 0.5;
Kansuni 0:5486cc47e272 139 float RIGHT1_pow = 0.5;
Kansuni 0:5486cc47e272 140 float RIGHT2_pow = 0.5;
Kansuni 0:5486cc47e272 141
Kansuni 0:5486cc47e272 142 x1 = (x - 0.5) * 2.0; //-1 < x1 < 1
Kansuni 0:5486cc47e272 143 x1 = SizeDown(x1, 1.0);
Kansuni 0:5486cc47e272 144 x_left = x1 + 1.0;
Kansuni 0:5486cc47e272 145 x_left = SizeDown(x_left, 1.0); //0 < x_left < 1
Kansuni 0:5486cc47e272 146 x_right = -x1 + 1.0;
Kansuni 0:5486cc47e272 147 x_right = SizeDown(x_right, 1.0); //0 < x_right < 1
Kansuni 0:5486cc47e272 148 y1 = (y - 0.5) * 2.0; //-1 < y1 < 1
Kansuni 0:5486cc47e272 149 y1 = SizeDown(y1, 1.0);
Kansuni 0:5486cc47e272 150 V_amount = ( (x1*x1) + (y1*y1) ) / 1.41;
Kansuni 0:5486cc47e272 151 V_amount = SizeDown(V_amount, 1.0);
Kansuni 0:5486cc47e272 152
Kansuni 0:5486cc47e272 153 //移動用モータ（前方向移動時）の調整
Kansuni 0:5486cc47e272 154 if(y1 >= 0.0 && y1 <= 1.0){
Kansuni 0:5486cc47e272 155 LEFT1_pow = ((x_left * V_amount)/2.0) + 0.5;
Kansuni 0:5486cc47e272 156 if(LEFT1_pow >= 1.0){
Kansuni 0:5486cc47e272 157 LEFT1_pow = 1.0;
Kansuni 0:5486cc47e272 158 }
Kansuni 0:5486cc47e272 159 else if(LEFT1_pow <= 0.0){
Kansuni 0:5486cc47e272 160 LEFT1_pow = 0.0;
Kansuni 0:5486cc47e272 161 }
Kansuni 0:5486cc47e272 162 motor_LEFT1 = LEFT1_pow;
Kansuni 0:5486cc47e272 163 LEFT2_pow = ((x_left * (-V_amount))/2.0) + 0.5;
Kansuni 0:5486cc47e272 164 if(LEFT2_pow >= 1.0){
Kansuni 0:5486cc47e272 165 LEFT2_pow = 1.0;
Kansuni 0:5486cc47e272 166 }
Kansuni 0:5486cc47e272 167 else if(LEFT2_pow <= 0.0){
Kansuni 0:5486cc47e272 168 LEFT2_pow = 0.0;
Kansuni 0:5486cc47e272 169 }
Kansuni 0:5486cc47e272 170 motor_LEFT2 = LEFT2_pow;
Kansuni 0:5486cc47e272 171
Kansuni 0:5486cc47e272 172 RIGHT1_pow = ((x_right * (-V_amount))/2.0) + 0.5;
Kansuni 0:5486cc47e272 173 if(RIGHT1_pow >= 1.0){
Kansuni 0:5486cc47e272 174 RIGHT1_pow = 1.0;
Kansuni 0:5486cc47e272 175 }
Kansuni 0:5486cc47e272 176 else if(RIGHT1_pow <= 0.0){
Kansuni 0:5486cc47e272 177 RIGHT1_pow = 0.0;
Kansuni 0:5486cc47e272 178 }
Kansuni 0:5486cc47e272 179 motor_RIGHT1 = RIGHT1_pow;
Kansuni 0:5486cc47e272 180 RIGHT2_pow = ((x_right * V_amount)/2.0) + 0.5;
Kansuni 0:5486cc47e272 181 if(RIGHT2_pow >= 1.0){
Kansuni 0:5486cc47e272 182 RIGHT2_pow = 1.0;
Kansuni 0:5486cc47e272 183 }
Kansuni 0:5486cc47e272 184 else if(RIGHT2_pow <= 0.0){
Kansuni 0:5486cc47e272 185 RIGHT2_pow = 0.0;
Kansuni 0:5486cc47e272 186 }
Kansuni 0:5486cc47e272 187 motor_RIGHT2 = RIGHT2_pow;
Kansuni 0:5486cc47e272 188 }
Kansuni 0:5486cc47e272 189 else{ //移動用モータ（後ろ方向移動時）の調整
Kansuni 0:5486cc47e272 190 LEFT1_pow = ((x_left * (-V_amount))/2.0) + 0.5;
Kansuni 0:5486cc47e272 191 if(LEFT1_pow >= 1.0){
Kansuni 0:5486cc47e272 192 LEFT1_pow = 1.0;
Kansuni 0:5486cc47e272 193 }
Kansuni 0:5486cc47e272 194 else if(LEFT1_pow <= 0.0){
Kansuni 0:5486cc47e272 195 LEFT1_pow = 0.0;
Kansuni 0:5486cc47e272 196 }
Kansuni 0:5486cc47e272 197 motor_LEFT1 = LEFT1_pow;
Kansuni 0:5486cc47e272 198 LEFT2_pow = ((x_left * V_amount)/2.0) + 0.5;
Kansuni 0:5486cc47e272 199 if(LEFT2_pow >= 1.0){
Kansuni 0:5486cc47e272 200 LEFT2_pow = 1.0;
Kansuni 0:5486cc47e272 201 }
Kansuni 0:5486cc47e272 202 else if(LEFT2_pow <= 0.0){
Kansuni 0:5486cc47e272 203 LEFT2_pow = 0.0;
Kansuni 0:5486cc47e272 204 }
Kansuni 0:5486cc47e272 205 motor_LEFT2 = LEFT2_pow;
Kansuni 0:5486cc47e272 206
Kansuni 0:5486cc47e272 207 RIGHT1_pow = ((x_right * V_amount)/2.0) + 0.5;
Kansuni 0:5486cc47e272 208 if(RIGHT1_pow >= 1.0){
Kansuni 0:5486cc47e272 209 RIGHT1_pow = 1.0;
Kansuni 0:5486cc47e272 210 }
Kansuni 0:5486cc47e272 211 else if(RIGHT1_pow <= 0.0){
Kansuni 0:5486cc47e272 212 RIGHT1_pow = 0.0;
Kansuni 0:5486cc47e272 213 }
Kansuni 0:5486cc47e272 214 motor_RIGHT1 = RIGHT1_pow;
Kansuni 0:5486cc47e272 215 RIGHT2_pow = ((x_right * (-V_amount))/2.0) + 0.5;
Kansuni 0:5486cc47e272 216 if(RIGHT2_pow >= 1.0){
Kansuni 0:5486cc47e272 217 RIGHT2_pow = 1.0;
Kansuni 0:5486cc47e272 218 }
Kansuni 0:5486cc47e272 219 else if(RIGHT2_pow <= 0.0){
Kansuni 0:5486cc47e272 220 RIGHT2_pow = 0.0;
Kansuni 0:5486cc47e272 221 }
Kansuni 0:5486cc47e272 222 motor_RIGHT2 = RIGHT2_pow;
Kansuni 0:5486cc47e272 223 }
Kansuni 0:5486cc47e272 224
Kansuni 0:5486cc47e272 225 wait(0.01);
Kansuni 0:5486cc47e272 226 }
Kansuni 0:5486cc47e272 227
Kansuni 0:5486cc47e272 228 //車体後部リフトの上下操作のコントロール
Kansuni 0:5486cc47e272 229 void Rifting_control(){
Kansuni 0:5486cc47e272 230
Kansuni 0:5486cc47e272 231 //Aボタンが押されていれば、リフトを上に移動
Kansuni 0:5486cc47e272 232 if(Abutton == 1 && Bbutton == 0){
Kansuni 0:5486cc47e272 233 //Debug.printf("RiftUP \n");
Kansuni 0:5486cc47e272 234 motor_Rift1 = 0;
Kansuni 0:5486cc47e272 235 motor_Rift2 = 1;
Kansuni 0:5486cc47e272 236 wait(0.01);
Kansuni 0:5486cc47e272 237 }
Kansuni 0:5486cc47e272 238 //Bボタンが押されていれば、リフトを下に移動
Kansuni 0:5486cc47e272 239 else if(Abutton == 0 && Bbutton == 1){
Kansuni 0:5486cc47e272 240 //Debug.printf("RiftDOWN \n");
Kansuni 0:5486cc47e272 241 motor_Rift1 = 1;
Kansuni 0:5486cc47e272 242 motor_Rift2 = 0;
Kansuni 0:5486cc47e272 243 wait(0.01);
Kansuni 0:5486cc47e272 244 }
Kansuni 0:5486cc47e272 245 //それ以外は停止
Kansuni 0:5486cc47e272 246 else{
Kansuni 0:5486cc47e272 247 motor_Rift1 = 1;
Kansuni 0:5486cc47e272 248 motor_Rift2 = 1;
Kansuni 0:5486cc47e272 249 wait(0.01);
Kansuni 0:5486cc47e272 250 }
Kansuni 0:5486cc47e272 251 }
Kansuni 0:5486cc47e272 252
Kansuni 0:5486cc47e272 253 //シリンダーの開閉操作をコントロール
Kansuni 0:5486cc47e272 254 void Cylinder_control(){
Kansuni 0:5486cc47e272 255
Kansuni 0:5486cc47e272 256 //フラグが0の状態でXボタンが押されれば、シリンダーを開放、コップ固定用出っ張りを開放
Kansuni 0:5486cc47e272 257 if(Xbutton == 1 && Cylinder_toggle == 0 && Cylinder_Open_Standby == 1){
Kansuni 0:5486cc47e272 258 //シリンダーを開放
Kansuni 0:5486cc47e272 259 //Debug.printf("Cylinder Open \n");
Kansuni 0:5486cc47e272 260 valve = 1;
Kansuni 0:5486cc47e272 261 Cylinder_toggle = 1;
Kansuni 0:5486cc47e272 262 Cylinder_Open_Standby = 0;
Kansuni 0:5486cc47e272 263 Cylinder_Close_Flag = 1;
Kansuni 0:5486cc47e272 264 //0.5秒間待機
Kansuni 0:5486cc47e272 265 wait(0.5);
Kansuni 0:5486cc47e272 266 //ストッパー（ウォームギア）を0.15秒間正回転
Kansuni 0:5486cc47e272 267 motor_Stopper1 = 1;
Kansuni 0:5486cc47e272 268 motor_Stopper2 = 0;
Kansuni 0:5486cc47e272 269 wait(0.15);
Kansuni 0:5486cc47e272 270 //ストッパー（ウォームギア）を停止
Kansuni 0:5486cc47e272 271 motor_Stopper1 = 1;
Kansuni 0:5486cc47e272 272 motor_Stopper2 = 1;
Kansuni 0:5486cc47e272 273 wait(0.05);
Kansuni 0:5486cc47e272 274 }
Kansuni 0:5486cc47e272 275 //フラグが1の状態でXボタンが押されれば、シリンダーを収納、コップ固定用出っ張りを収納
Kansuni 0:5486cc47e272 276 //スタンバイフラグが1になるタイミングは、while文がIteration回だけ反復した時
Kansuni 0:5486cc47e272 277 else if(Xbutton == 1 && Cylinder_toggle == 1 && Cylinder_Close_Standby == 1){
Kansuni 0:5486cc47e272 278 //シリンダーを収納
Kansuni 0:5486cc47e272 279 //Debug.printf("Cylinder Close \n");
Kansuni 0:5486cc47e272 280 valve = 0;
Kansuni 0:5486cc47e272 281 Cylinder_toggle = 0;
Kansuni 0:5486cc47e272 282 Cylinder_Close_Standby = 0;
Kansuni 0:5486cc47e272 283 Cylinder_Open_Flag = 1;
Kansuni 0:5486cc47e272 284 wait(0.01);
Kansuni 0:5486cc47e272 285 //0.5秒間待機
Kansuni 0:5486cc47e272 286 wait(0.5);
Kansuni 0:5486cc47e272 287 //ストッパー（ウォームギア）を0.15秒間逆回転
Kansuni 0:5486cc47e272 288 motor_Stopper1 = 0;
Kansuni 0:5486cc47e272 289 motor_Stopper2 = 1;
Kansuni 0:5486cc47e272 290 wait(0.15);
Kansuni 0:5486cc47e272 291 //ストッパー（ウォームギア）を停止
Kansuni 0:5486cc47e272 292 motor_Stopper1 = 1;
Kansuni 0:5486cc47e272 293 motor_Stopper2 = 1;
Kansuni 0:5486cc47e272 294 wait(0.05);
Kansuni 0:5486cc47e272 295 }
Kansuni 0:5486cc47e272 296 }
Kansuni 0:5486cc47e272 297
Kansuni 0:5486cc47e272 298 //特定の範囲内にパラメータを抑え込む関数(widthは正数で定義)
Kansuni 0:5486cc47e272 299 float SizeDown(float param, float width){
Kansuni 0:5486cc47e272 300 if(param >= width){
Kansuni 0:5486cc47e272 301 param = width;
Kansuni 0:5486cc47e272 302 }
Kansuni 0:5486cc47e272 303 else if(param <= -width){
Kansuni 0:5486cc47e272 304 param = -width;
Kansuni 0:5486cc47e272 305 }
Kansuni 0:5486cc47e272 306 return param;
Kansuni 0:5486cc47e272 307 }