LabRobocon / Mbed 2 deprecated LabRobocon_2017

Added one task

Dependencies:   mbed

main.cpp@0:fb4269aa5fb4, 2017-05-26 (annotated)

Committer:
PicYusuke
Date:
Fri May 26 03:51:19 2017 +0000
Revision:
0:fb4269aa5fb4
hoge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PicYusuke 0:fb4269aa5fb4 1 #include "mbed.h"
PicYusuke 0:fb4269aa5fb4 2
PicYusuke 0:fb4269aa5fb4 3 #include "gpio.h"
PicYusuke 0:fb4269aa5fb4 4
PicYusuke 0:fb4269aa5fb4 5 #include "robot.h"
PicYusuke 0:fb4269aa5fb4 6 #include "GP2Y0E03.h"
PicYusuke 0:fb4269aa5fb4 7 #include "MCP3208.h"
PicYusuke 0:fb4269aa5fb4 8 #include "L3GD20.h"
PicYusuke 0:fb4269aa5fb4 9
PicYusuke 0:fb4269aa5fb4 10 #define ERROR (-1)
PicYusuke 0:fb4269aa5fb4 11 #define SUCCESS 0
PicYusuke 0:fb4269aa5fb4 12
PicYusuke 0:fb4269aa5fb4 13 //#define L3GD20_TEST
PicYusuke 0:fb4269aa5fb4 14 //#define GP2Y0E03_TEST
PicYusuke 0:fb4269aa5fb4 15 //#define MCP3208_TEST
PicYusuke 0:fb4269aa5fb4 16 //#define LBR127_TEST
PicYusuke 0:fb4269aa5fb4 17
PicYusuke 0:fb4269aa5fb4 18 /*---------RCサーボ制御用--------*/
PicYusuke 0:fb4269aa5fb4 19 float servo_pulse = 0.001f; //RCサーボパルス幅[s]
PicYusuke 0:fb4269aa5fb4 20 uint32_t servo_state = 0; //サーボピン状態
PicYusuke 0:fb4269aa5fb4 21 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 22
PicYusuke 0:fb4269aa5fb4 23 /*-----------------ラインセンサ--------------------*/
PicYusuke 0:fb4269aa5fb4 24 float line_pos[2] = {0.0f, 0.0f};
PicYusuke 0:fb4269aa5fb4 25 uint32_t intersection = 0; //交差点検知
PicYusuke 0:fb4269aa5fb4 26 /*------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 27
PicYusuke 0:fb4269aa5fb4 28 /*---------状態変数----------*/
PicYusuke 0:fb4269aa5fb4 29 float v_tr = 0.0f; //並進方向電圧, unit: V
PicYusuke 0:fb4269aa5fb4 30 float theta = 0.0f; //姿勢角, unit: degree
PicYusuke 0:fb4269aa5fb4 31
PicYusuke 0:fb4269aa5fb4 32 float omega_d = 0.0f; //目標角速度, unit: degree/s
PicYusuke 0:fb4269aa5fb4 33 /*--------------------------*/
PicYusuke 0:fb4269aa5fb4 34
PicYusuke 0:fb4269aa5fb4 35 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 36 float ts = 0.001f; //制御周期
PicYusuke 0:fb4269aa5fb4 37
PicYusuke 0:fb4269aa5fb4 38 uint32_t sequence = 0; //タスク管理変数
PicYusuke 0:fb4269aa5fb4 39 uint32_t error_seq = 0xFF; //エラーを起こしているタスク番号
PicYusuke 0:fb4269aa5fb4 40 int32_t (*Task_Fptr[10])(void); //タスク管理用関数ポインタ
PicYusuke 0:fb4269aa5fb4 41 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 42
PicYusuke 0:fb4269aa5fb4 43 /*-----制御器-----*/
PicYusuke 0:fb4269aa5fb4 44 PID_Struct trace_pid; //ライントレース用
PicYusuke 0:fb4269aa5fb4 45 PID_Struct gyro_pid; //ジャイロによるオドメトリ用
PicYusuke 0:fb4269aa5fb4 46
PicYusuke 0:fb4269aa5fb4 47 PID_Struct manual_control; //手動モータ制御用
PicYusuke 0:fb4269aa5fb4 48
PicYusuke 0:fb4269aa5fb4 49 PID_Struct *current_pid_ptr; //現在接続されている制御器
PicYusuke 0:fb4269aa5fb4 50 /*----------------*/
PicYusuke 0:fb4269aa5fb4 51
PicYusuke 0:fb4269aa5fb4 52 /*---------各タスク関数--------*/
PicYusuke 0:fb4269aa5fb4 53 //ロボットの動作をシーケンシャルに記述する
PicYusuke 0:fb4269aa5fb4 54 //各タスク関数内ではフィードバック制御則へのアクセス原則禁止
PicYusuke 0:fb4269aa5fb4 55 int32_t Task0(void);
PicYusuke 0:fb4269aa5fb4 56 int32_t Task1(void);
PicYusuke 0:fb4269aa5fb4 57 int32_t Task2(void);
PicYusuke 0:fb4269aa5fb4 58 int32_t Task3(void);
PicYusuke 0:fb4269aa5fb4 59 int32_t Task4(void);
PicYusuke 0:fb4269aa5fb4 60 int32_t Task5(void);
PicYusuke 0:fb4269aa5fb4 61 int32_t Task6(void);
PicYusuke 0:fb4269aa5fb4 62 int32_t Task7(void);
PicYusuke 0:fb4269aa5fb4 63 int32_t Task8(void);
PicYusuke 0:fb4269aa5fb4 64 int32_t Task9(void);
PicYusuke 0:fb4269aa5fb4 65 /*---------------------------*/
PicYusuke 0:fb4269aa5fb4 66
PicYusuke 0:fb4269aa5fb4 67 void Turn_90deg(int32_t);
PicYusuke 0:fb4269aa5fb4 68 void Turn_180deg(int32_t);
PicYusuke 0:fb4269aa5fb4 69
PicYusuke 0:fb4269aa5fb4 70 void Accelerate(float);
PicYusuke 0:fb4269aa5fb4 71 void Decelerate();
PicYusuke 0:fb4269aa5fb4 72 void Stop();
PicYusuke 0:fb4269aa5fb4 73
PicYusuke 0:fb4269aa5fb4 74 int main()
PicYusuke 0:fb4269aa5fb4 75 {
PicYusuke 0:fb4269aa5fb4 76 uint32_t i;
PicYusuke 0:fb4269aa5fb4 77
PicYusuke 0:fb4269aa5fb4 78 /*Peripheral initialization*/
PicYusuke 0:fb4269aa5fb4 79 //GPIO_Init();
PicYusuke 0:fb4269aa5fb4 80
PicYusuke 0:fb4269aa5fb4 81 SPI_Init();
PicYusuke 0:fb4269aa5fb4 82
PicYusuke 0:fb4269aa5fb4 83 /*-----各種デバイス初期化----*/
PicYusuke 0:fb4269aa5fb4 84 L3GD20_Init();
PicYusuke 0:fb4269aa5fb4 85 MCP3208_Init();
PicYusuke 0:fb4269aa5fb4 86 /*-------------------------*/
PicYusuke 0:fb4269aa5fb4 87
PicYusuke 0:fb4269aa5fb4 88 /*------フィードバックコントローラ-------*/
PicYusuke 0:fb4269aa5fb4 89 PID_Struct_Init(&trace_pid, 1.1, 20.0, 0.0);
PicYusuke 0:fb4269aa5fb4 90 //PID_Struct_Init(&gyro_pid, 0.015, 0.0, 0.0);
PicYusuke 0:fb4269aa5fb4 91 PID_Struct_Init(&gyro_pid, 0.02, 0.0, 0.0);
PicYusuke 0:fb4269aa5fb4 92 current_pid_ptr = &manual_control;
PicYusuke 0:fb4269aa5fb4 93 /*-----------------------------------*/
PicYusuke 0:fb4269aa5fb4 94
PicYusuke 0:fb4269aa5fb4 95 TIM_Init(); //タイマスタート
PicYusuke 0:fb4269aa5fb4 96 /*-------------------------*/
PicYusuke 0:fb4269aa5fb4 97
PicYusuke 0:fb4269aa5fb4 98 /*-----関数ポインタ設定------*/
PicYusuke 0:fb4269aa5fb4 99 Task_Fptr[0] = Task0;
PicYusuke 0:fb4269aa5fb4 100 Task_Fptr[1] = Task1;
PicYusuke 0:fb4269aa5fb4 101 Task_Fptr[2] = Task2;
PicYusuke 0:fb4269aa5fb4 102 Task_Fptr[3] = Task3;
PicYusuke 0:fb4269aa5fb4 103 Task_Fptr[4] = Task4;
PicYusuke 0:fb4269aa5fb4 104 Task_Fptr[5] = Task5;
PicYusuke 0:fb4269aa5fb4 105 Task_Fptr[6] = Task6;
PicYusuke 0:fb4269aa5fb4 106 Task_Fptr[7] = Task7;
PicYusuke 0:fb4269aa5fb4 107 Task_Fptr[8] = Task8;
PicYusuke 0:fb4269aa5fb4 108 Task_Fptr[9] = Task9;
PicYusuke 0:fb4269aa5fb4 109 /*-------------------------*/
PicYusuke 0:fb4269aa5fb4 110
PicYusuke 0:fb4269aa5fb4 111
PicYusuke 0:fb4269aa5fb4 112
PicYusuke 0:fb4269aa5fb4 113
PicYusuke 0:fb4269aa5fb4 114 while(1)
PicYusuke 0:fb4269aa5fb4 115 {
PicYusuke 0:fb4269aa5fb4 116 if(Task_Fptr[sequence]() == ERROR) //タスク呼び出し, エラーチェック
PicYusuke 0:fb4269aa5fb4 117 {
PicYusuke 0:fb4269aa5fb4 118 error_seq = sequence; //エラーを起こしているタスク番号を取得
PicYusuke 0:fb4269aa5fb4 119 }
PicYusuke 0:fb4269aa5fb4 120
PicYusuke 0:fb4269aa5fb4 121 sequence ++;
PicYusuke 0:fb4269aa5fb4 122 /*
PicYusuke 0:fb4269aa5fb4 123 if(sequence == 1)
PicYusuke 0:fb4269aa5fb4 124 {
PicYusuke 0:fb4269aa5fb4 125 sequence = 3;
PicYusuke 0:fb4269aa5fb4 126 }
PicYusuke 0:fb4269aa5fb4 127 */
PicYusuke 0:fb4269aa5fb4 128 }
PicYusuke 0:fb4269aa5fb4 129 }
PicYusuke 0:fb4269aa5fb4 130
PicYusuke 0:fb4269aa5fb4 131 int32_t Task0(void)
PicYusuke 0:fb4269aa5fb4 132 {
PicYusuke 0:fb4269aa5fb4 133 /*
PicYusuke 0:fb4269aa5fb4 134 if()
PicYusuke 0:fb4269aa5fb4 135 {
PicYusuke 0:fb4269aa5fb4 136 return ERROR;
PicYusuke 0:fb4269aa5fb4 137 }
PicYusuke 0:fb4269aa5fb4 138 */
PicYusuke 0:fb4269aa5fb4 139
PicYusuke 0:fb4269aa5fb4 140 servo_pulse = 0.0021f; //アームを折りたたむ
PicYusuke 0:fb4269aa5fb4 141 wait(0.5f);
PicYusuke 0:fb4269aa5fb4 142
PicYusuke 0:fb4269aa5fb4 143 /*-----最下点までリフトを移動-----*/
PicYusuke 0:fb4269aa5fb4 144 Put_Vm_Lift(-0.8f); //リフト下降
PicYusuke 0:fb4269aa5fb4 145 wait(2.0f);
PicYusuke 0:fb4269aa5fb4 146 Put_Vm_Lift(0.0f); //リフト停止
PicYusuke 0:fb4269aa5fb4 147 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 148 /*----リフトのリポジショニング----*/
PicYusuke 0:fb4269aa5fb4 149 Put_Vm_Lift(1.0f); //リフト上昇(どん兵衛用)
PicYusuke 0:fb4269aa5fb4 150 wait(0.8f);
PicYusuke 0:fb4269aa5fb4 151 Put_Vm_Lift(0.0f); //リフト停止
PicYusuke 0:fb4269aa5fb4 152 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 153
PicYusuke 0:fb4269aa5fb4 154 /*--------------------スタート待機-----------------------*/
PicYusuke 0:fb4269aa5fb4 155 while(ain_cup > 0.5f) //カップ検出センサが反応するまで待機
PicYusuke 0:fb4269aa5fb4 156 {
PicYusuke 0:fb4269aa5fb4 157 printf("cup = %f\n", (float)ain_cup);
PicYusuke 0:fb4269aa5fb4 158 }
PicYusuke 0:fb4269aa5fb4 159
PicYusuke 0:fb4269aa5fb4 160 servo_pulse = 0.001f; //アームを展開
PicYusuke 0:fb4269aa5fb4 161 wait(0.3f);
PicYusuke 0:fb4269aa5fb4 162 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 163 /*
PicYusuke 0:fb4269aa5fb4 164 while(1)
PicYusuke 0:fb4269aa5fb4 165 {
PicYusuke 0:fb4269aa5fb4 166 HAL_Delay(10);
PicYusuke 0:fb4269aa5fb4 167 printf("pos = %f\n", line_pos[0]);
PicYusuke 0:fb4269aa5fb4 168 }
PicYusuke 0:fb4269aa5fb4 169 */
PicYusuke 0:fb4269aa5fb4 170 return 0;
PicYusuke 0:fb4269aa5fb4 171 }
PicYusuke 0:fb4269aa5fb4 172
PicYusuke 0:fb4269aa5fb4 173 int32_t Task1(void)
PicYusuke 0:fb4269aa5fb4 174 {
PicYusuke 0:fb4269aa5fb4 175 uint32_t i;
PicYusuke 0:fb4269aa5fb4 176 float v = 0.0f;
PicYusuke 0:fb4269aa5fb4 177
PicYusuke 0:fb4269aa5fb4 178 /*
PicYusuke 0:fb4269aa5fb4 179 if()
PicYusuke 0:fb4269aa5fb4 180 {
PicYusuke 0:fb4269aa5fb4 181 return ERROR;
PicYusuke 0:fb4269aa5fb4 182 }
PicYusuke 0:fb4269aa5fb4 183 */
PicYusuke 0:fb4269aa5fb4 184
PicYusuke 0:fb4269aa5fb4 185 /*-------------------ライントレース--------------------*/
PicYusuke 0:fb4269aa5fb4 186 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 187 Accelerate(2.0f); //加速
PicYusuke 0:fb4269aa5fb4 188
PicYusuke 0:fb4269aa5fb4 189 for(i = 0; i < 2; i ++) //1-2回目の交差点はスキップ
PicYusuke 0:fb4269aa5fb4 190 {
PicYusuke 0:fb4269aa5fb4 191 while(!intersection) //一回目の交差点検出はスキップ
PicYusuke 0:fb4269aa5fb4 192 {
PicYusuke 0:fb4269aa5fb4 193 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 194 }
PicYusuke 0:fb4269aa5fb4 195 while(intersection) //一回目の交差点通過を待つ
PicYusuke 0:fb4269aa5fb4 196 {
PicYusuke 0:fb4269aa5fb4 197 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 198 }
PicYusuke 0:fb4269aa5fb4 199 }
PicYusuke 0:fb4269aa5fb4 200 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 201
PicYusuke 0:fb4269aa5fb4 202 return 0;
PicYusuke 0:fb4269aa5fb4 203 }
PicYusuke 0:fb4269aa5fb4 204
PicYusuke 0:fb4269aa5fb4 205 int32_t Task2(void) //3本目の交差点を左折
PicYusuke 0:fb4269aa5fb4 206 {
PicYusuke 0:fb4269aa5fb4 207 /*
PicYusuke 0:fb4269aa5fb4 208 if()
PicYusuke 0:fb4269aa5fb4 209 {
PicYusuke 0:fb4269aa5fb4 210 return ERROR;
PicYusuke 0:fb4269aa5fb4 211 }
PicYusuke 0:fb4269aa5fb4 212 */
PicYusuke 0:fb4269aa5fb4 213
PicYusuke 0:fb4269aa5fb4 214 while(!intersection) //3回目の交差点検出まで直進
PicYusuke 0:fb4269aa5fb4 215 {
PicYusuke 0:fb4269aa5fb4 216 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 217 }
PicYusuke 0:fb4269aa5fb4 218 while(intersection) //3回目の交差点通過を待つ
PicYusuke 0:fb4269aa5fb4 219 {
PicYusuke 0:fb4269aa5fb4 220 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 221 }
PicYusuke 0:fb4269aa5fb4 222
PicYusuke 0:fb4269aa5fb4 223 wait(0.4f);
PicYusuke 0:fb4269aa5fb4 224
PicYusuke 0:fb4269aa5fb4 225 Stop(); //停止
PicYusuke 0:fb4269aa5fb4 226 Turn_90deg(1); //左90度旋回
PicYusuke 0:fb4269aa5fb4 227
PicYusuke 0:fb4269aa5fb4 228 return 0;
PicYusuke 0:fb4269aa5fb4 229 }
PicYusuke 0:fb4269aa5fb4 230
PicYusuke 0:fb4269aa5fb4 231 int32_t Task3(void) //横道を直進し、どん兵衛を回収、180度回転
PicYusuke 0:fb4269aa5fb4 232 {
PicYusuke 0:fb4269aa5fb4 233 uint32_t i;
PicYusuke 0:fb4269aa5fb4 234 float v = 0.0f;
PicYusuke 0:fb4269aa5fb4 235
PicYusuke 0:fb4269aa5fb4 236 /*
PicYusuke 0:fb4269aa5fb4 237 if()
PicYusuke 0:fb4269aa5fb4 238 {
PicYusuke 0:fb4269aa5fb4 239 return ERROR;
PicYusuke 0:fb4269aa5fb4 240 }
PicYusuke 0:fb4269aa5fb4 241 */
PicYusuke 0:fb4269aa5fb4 242
PicYusuke 0:fb4269aa5fb4 243 /*-------------------ライントレース--------------------*/
PicYusuke 0:fb4269aa5fb4 244 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 245 Accelerate(1.0f); //加速
PicYusuke 0:fb4269aa5fb4 246 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 247
PicYusuke 0:fb4269aa5fb4 248 #if 0
PicYusuke 0:fb4269aa5fb4 249 //本来はここでカップを検出するまで前進する
PicYusuke 0:fb4269aa5fb4 250 wait(1.3f);
PicYusuke 0:fb4269aa5fb4 251 #endif
PicYusuke 0:fb4269aa5fb4 252
PicYusuke 0:fb4269aa5fb4 253 #if 1
PicYusuke 0:fb4269aa5fb4 254 while(ain_cup > 0.6f) //カップ検出まで前進
PicYusuke 0:fb4269aa5fb4 255 {
PicYusuke 0:fb4269aa5fb4 256 printf("waiting...\n");
PicYusuke 0:fb4269aa5fb4 257 }
PicYusuke 0:fb4269aa5fb4 258
PicYusuke 0:fb4269aa5fb4 259 wait(0.05f);
PicYusuke 0:fb4269aa5fb4 260 Stop(); //停止
PicYusuke 0:fb4269aa5fb4 261
PicYusuke 0:fb4269aa5fb4 262 servo_pulse = 0.0021f; //カップ把持
PicYusuke 0:fb4269aa5fb4 263 wait(1.0f);
PicYusuke 0:fb4269aa5fb4 264
PicYusuke 0:fb4269aa5fb4 265 Put_Vm_Lift(1.5f); //リフト上昇
PicYusuke 0:fb4269aa5fb4 266 wait(0.8f);
PicYusuke 0:fb4269aa5fb4 267 Put_Vm_Lift(0.0f); //リフト停止
PicYusuke 0:fb4269aa5fb4 268 #endif
PicYusuke 0:fb4269aa5fb4 269
PicYusuke 0:fb4269aa5fb4 270 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 271 Accelerate(1.5f); //加速
PicYusuke 0:fb4269aa5fb4 272 wait(0.5f);
PicYusuke 0:fb4269aa5fb4 273 Stop(); //停止
PicYusuke 0:fb4269aa5fb4 274
PicYusuke 0:fb4269aa5fb4 275 Turn_180deg(1); //左180度旋回
PicYusuke 0:fb4269aa5fb4 276
PicYusuke 0:fb4269aa5fb4 277 return 0;
PicYusuke 0:fb4269aa5fb4 278 }
PicYusuke 0:fb4269aa5fb4 279
PicYusuke 0:fb4269aa5fb4 280 int32_t Task4(void) //カップ回収後、メインストリートに戻る
PicYusuke 0:fb4269aa5fb4 281 {
PicYusuke 0:fb4269aa5fb4 282 uint32_t i;
PicYusuke 0:fb4269aa5fb4 283 float v = 0.0f;
PicYusuke 0:fb4269aa5fb4 284
PicYusuke 0:fb4269aa5fb4 285 /*
PicYusuke 0:fb4269aa5fb4 286 if()
PicYusuke 0:fb4269aa5fb4 287 {
PicYusuke 0:fb4269aa5fb4 288 return ERROR;
PicYusuke 0:fb4269aa5fb4 289 }
PicYusuke 0:fb4269aa5fb4 290 */
PicYusuke 0:fb4269aa5fb4 291
PicYusuke 0:fb4269aa5fb4 292 /*-------------------ライントレース--------------------*/
PicYusuke 0:fb4269aa5fb4 293 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 294 Accelerate(1.5f); //加速
PicYusuke 0:fb4269aa5fb4 295 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 296
PicYusuke 0:fb4269aa5fb4 297 while(!intersection) //交差点検出まで直進
PicYusuke 0:fb4269aa5fb4 298 {
PicYusuke 0:fb4269aa5fb4 299 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 300 }
PicYusuke 0:fb4269aa5fb4 301 while(intersection) //交差点通過を待つ
PicYusuke 0:fb4269aa5fb4 302 {
PicYusuke 0:fb4269aa5fb4 303 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 304 }
PicYusuke 0:fb4269aa5fb4 305
PicYusuke 0:fb4269aa5fb4 306 wait(0.6f);
PicYusuke 0:fb4269aa5fb4 307
PicYusuke 0:fb4269aa5fb4 308 Stop();
PicYusuke 0:fb4269aa5fb4 309
PicYusuke 0:fb4269aa5fb4 310 Turn_90deg(1); //左90度旋回
PicYusuke 0:fb4269aa5fb4 311
PicYusuke 0:fb4269aa5fb4 312 return 0;
PicYusuke 0:fb4269aa5fb4 313 }
PicYusuke 0:fb4269aa5fb4 314
PicYusuke 0:fb4269aa5fb4 315 int32_t Task5(void) //カップ回収後のメインストリート走行+カップ棚置き
PicYusuke 0:fb4269aa5fb4 316 {
PicYusuke 0:fb4269aa5fb4 317 uint32_t i;
PicYusuke 0:fb4269aa5fb4 318 float v = 0.0f;
PicYusuke 0:fb4269aa5fb4 319
PicYusuke 0:fb4269aa5fb4 320 /*
PicYusuke 0:fb4269aa5fb4 321 if()
PicYusuke 0:fb4269aa5fb4 322 {
PicYusuke 0:fb4269aa5fb4 323 return ERROR;
PicYusuke 0:fb4269aa5fb4 324 }
PicYusuke 0:fb4269aa5fb4 325 */
PicYusuke 0:fb4269aa5fb4 326 /*-------------------ライントレース--------------------*/
PicYusuke 0:fb4269aa5fb4 327 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 328 Accelerate(2.0f); //加速
PicYusuke 0:fb4269aa5fb4 329 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 330
PicYusuke 0:fb4269aa5fb4 331 while(ain_cup > 0.7f) //棚検出まで前進
PicYusuke 0:fb4269aa5fb4 332 {
PicYusuke 0:fb4269aa5fb4 333 printf("waiting...\n");
PicYusuke 0:fb4269aa5fb4 334 }
PicYusuke 0:fb4269aa5fb4 335
PicYusuke 0:fb4269aa5fb4 336 Stop();
PicYusuke 0:fb4269aa5fb4 337
PicYusuke 0:fb4269aa5fb4 338 servo_pulse = 0.001f; //カップ解放
PicYusuke 0:fb4269aa5fb4 339 wait(0.5f);
PicYusuke 0:fb4269aa5fb4 340
PicYusuke 0:fb4269aa5fb4 341 /*----------ジャイロを使って後退--------------*/
PicYusuke 0:fb4269aa5fb4 342 current_pid_ptr = &gyro_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 343 Accelerate(-1.5f); //加速
PicYusuke 0:fb4269aa5fb4 344
PicYusuke 0:fb4269aa5fb4 345 wait(0.6f);
PicYusuke 0:fb4269aa5fb4 346
PicYusuke 0:fb4269aa5fb4 347 Stop();
PicYusuke 0:fb4269aa5fb4 348 /*------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 349
PicYusuke 0:fb4269aa5fb4 350 Turn_180deg(1); //左180度旋回
PicYusuke 0:fb4269aa5fb4 351
PicYusuke 0:fb4269aa5fb4 352 /*-----最下点までリフトを移動-----*/
PicYusuke 0:fb4269aa5fb4 353 Put_Vm_Lift(-0.8f); //リフト下降
PicYusuke 0:fb4269aa5fb4 354 wait(2.5f);
PicYusuke 0:fb4269aa5fb4 355 Put_Vm_Lift(0.0f); //リフト停止
PicYusuke 0:fb4269aa5fb4 356 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 357 /*----リフトのリポジショニング----*/
PicYusuke 0:fb4269aa5fb4 358 Put_Vm_Lift(1.0f); //リフト上昇(UFO用)
PicYusuke 0:fb4269aa5fb4 359 wait(0.35f);
PicYusuke 0:fb4269aa5fb4 360 Put_Vm_Lift(0.0f); //リフト停止
PicYusuke 0:fb4269aa5fb4 361 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 362
PicYusuke 0:fb4269aa5fb4 363 return 0;
PicYusuke 0:fb4269aa5fb4 364 }
PicYusuke 0:fb4269aa5fb4 365
PicYusuke 0:fb4269aa5fb4 366 int32_t Task6(void) //4本目の交差点を棚側から左折
PicYusuke 0:fb4269aa5fb4 367 {
PicYusuke 0:fb4269aa5fb4 368
PicYusuke 0:fb4269aa5fb4 369 uint32_t i;
PicYusuke 0:fb4269aa5fb4 370 float v = 0;
PicYusuke 0:fb4269aa5fb4 371 /*
PicYusuke 0:fb4269aa5fb4 372 if()
PicYusuke 0:fb4269aa5fb4 373 {
PicYusuke 0:fb4269aa5fb4 374 return ERROR;
PicYusuke 0:fb4269aa5fb4 375 }
PicYusuke 0:fb4269aa5fb4 376 */
PicYusuke 0:fb4269aa5fb4 377
PicYusuke 0:fb4269aa5fb4 378 /*-------------------ライントレース--------------------*/
PicYusuke 0:fb4269aa5fb4 379 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 380 Accelerate(1.5f); //加速
PicYusuke 0:fb4269aa5fb4 381 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 382
PicYusuke 0:fb4269aa5fb4 383 while(!intersection) //交差点検出まで直進
PicYusuke 0:fb4269aa5fb4 384 {
PicYusuke 0:fb4269aa5fb4 385 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 386 }
PicYusuke 0:fb4269aa5fb4 387 while(intersection) //交差点通過を待つ
PicYusuke 0:fb4269aa5fb4 388 {
PicYusuke 0:fb4269aa5fb4 389 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 390 }
PicYusuke 0:fb4269aa5fb4 391
PicYusuke 0:fb4269aa5fb4 392 wait(0.6f);
PicYusuke 0:fb4269aa5fb4 393
PicYusuke 0:fb4269aa5fb4 394 Stop(); //停止
PicYusuke 0:fb4269aa5fb4 395 Turn_90deg(1); //左90度旋回
PicYusuke 0:fb4269aa5fb4 396
PicYusuke 0:fb4269aa5fb4 397 return 0;
PicYusuke 0:fb4269aa5fb4 398 }
PicYusuke 0:fb4269aa5fb4 399
PicYusuke 0:fb4269aa5fb4 400 int32_t Task7(void) //横道を直進し、UFOを回収、180度回転
PicYusuke 0:fb4269aa5fb4 401 {
PicYusuke 0:fb4269aa5fb4 402 /*-------------------ライントレース--------------------*/
PicYusuke 0:fb4269aa5fb4 403 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 404 Accelerate(1.0f); //加速
PicYusuke 0:fb4269aa5fb4 405 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 406
PicYusuke 0:fb4269aa5fb4 407
PicYusuke 0:fb4269aa5fb4 408 while(ain_cup > 0.6f) //カップ検出まで前進
PicYusuke 0:fb4269aa5fb4 409 {
PicYusuke 0:fb4269aa5fb4 410 printf("waiting...\n");
PicYusuke 0:fb4269aa5fb4 411 }
PicYusuke 0:fb4269aa5fb4 412
PicYusuke 0:fb4269aa5fb4 413 wait(0.05f);
PicYusuke 0:fb4269aa5fb4 414 Stop(); //停止
PicYusuke 0:fb4269aa5fb4 415
PicYusuke 0:fb4269aa5fb4 416 servo_pulse = 0.0021f; //カップ把持
PicYusuke 0:fb4269aa5fb4 417 wait(1.0f);
PicYusuke 0:fb4269aa5fb4 418
PicYusuke 0:fb4269aa5fb4 419 Put_Vm_Lift(1.5f); //リフト上昇
PicYusuke 0:fb4269aa5fb4 420 wait(0.8f);
PicYusuke 0:fb4269aa5fb4 421 Put_Vm_Lift(0.0f); //リフト停止
PicYusuke 0:fb4269aa5fb4 422
PicYusuke 0:fb4269aa5fb4 423
PicYusuke 0:fb4269aa5fb4 424 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 425 Accelerate(1.5f); //加速
PicYusuke 0:fb4269aa5fb4 426 wait(0.5f);
PicYusuke 0:fb4269aa5fb4 427 Stop(); //停止
PicYusuke 0:fb4269aa5fb4 428
PicYusuke 0:fb4269aa5fb4 429 Turn_180deg(1); //左180度旋回
PicYusuke 0:fb4269aa5fb4 430 }
PicYusuke 0:fb4269aa5fb4 431
PicYusuke 0:fb4269aa5fb4 432
PicYusuke 0:fb4269aa5fb4 433 int32_t Task8(void) //棚左側にUFOを置くために、位置合わせ
PicYusuke 0:fb4269aa5fb4 434 {
PicYusuke 0:fb4269aa5fb4 435 uint32_t i;
PicYusuke 0:fb4269aa5fb4 436 float v = 0;
PicYusuke 0:fb4269aa5fb4 437 /*
PicYusuke 0:fb4269aa5fb4 438 if()
PicYusuke 0:fb4269aa5fb4 439 {
PicYusuke 0:fb4269aa5fb4 440 return ERROR;
PicYusuke 0:fb4269aa5fb4 441 }
PicYusuke 0:fb4269aa5fb4 442 */
PicYusuke 0:fb4269aa5fb4 443
PicYusuke 0:fb4269aa5fb4 444 /*-------------------ライントレース--------------------*/
PicYusuke 0:fb4269aa5fb4 445 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 446 Accelerate(1.5f); //加速
PicYusuke 0:fb4269aa5fb4 447 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 448
PicYusuke 0:fb4269aa5fb4 449 /*---------------1回目の交差点-----------------*/
PicYusuke 0:fb4269aa5fb4 450 while(!intersection) //交差点検出まで直進
PicYusuke 0:fb4269aa5fb4 451 {
PicYusuke 0:fb4269aa5fb4 452 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 453 }
PicYusuke 0:fb4269aa5fb4 454 while(intersection) //交差点通過を待つ
PicYusuke 0:fb4269aa5fb4 455 {
PicYusuke 0:fb4269aa5fb4 456 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 457 }
PicYusuke 0:fb4269aa5fb4 458
PicYusuke 0:fb4269aa5fb4 459 wait(2.5f); //棚左側とカップの位置を合わせるためのパラメータ
PicYusuke 0:fb4269aa5fb4 460 /*--------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 461
PicYusuke 0:fb4269aa5fb4 462 Stop();
PicYusuke 0:fb4269aa5fb4 463 Turn_90deg(-1); //右90度旋回
PicYusuke 0:fb4269aa5fb4 464
PicYusuke 0:fb4269aa5fb4 465 return 0;
PicYusuke 0:fb4269aa5fb4 466 }
PicYusuke 0:fb4269aa5fb4 467
PicYusuke 0:fb4269aa5fb4 468 int32_t Task9(void) //ジャイロを使って棚まで接近、UFOを置く
PicYusuke 0:fb4269aa5fb4 469 {
PicYusuke 0:fb4269aa5fb4 470 uint32_t i;
PicYusuke 0:fb4269aa5fb4 471 float v = 0;
PicYusuke 0:fb4269aa5fb4 472 /*
PicYusuke 0:fb4269aa5fb4 473 if()
PicYusuke 0:fb4269aa5fb4 474 {
PicYusuke 0:fb4269aa5fb4 475 return ERROR;
PicYusuke 0:fb4269aa5fb4 476 }
PicYusuke 0:fb4269aa5fb4 477 */
PicYusuke 0:fb4269aa5fb4 478
PicYusuke 0:fb4269aa5fb4 479 /*----------ジャイロを使って前進--------------*/
PicYusuke 0:fb4269aa5fb4 480 current_pid_ptr = &gyro_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 481 Accelerate(1.5f); //加速
PicYusuke 0:fb4269aa5fb4 482 /*------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 483
PicYusuke 0:fb4269aa5fb4 484 while(ain_cup > 0.7f) //棚検出まで前進
PicYusuke 0:fb4269aa5fb4 485 {
PicYusuke 0:fb4269aa5fb4 486 printf("waiting...\n");
PicYusuke 0:fb4269aa5fb4 487 }
PicYusuke 0:fb4269aa5fb4 488
PicYusuke 0:fb4269aa5fb4 489 Stop();
PicYusuke 0:fb4269aa5fb4 490
PicYusuke 0:fb4269aa5fb4 491 servo_pulse = 0.001f; //カップ解放
PicYusuke 0:fb4269aa5fb4 492 wait(0.5f);
PicYusuke 0:fb4269aa5fb4 493
PicYusuke 0:fb4269aa5fb4 494 while(1)
PicYusuke 0:fb4269aa5fb4 495 {
PicYusuke 0:fb4269aa5fb4 496 printf("Tasks completed\n");
PicYusuke 0:fb4269aa5fb4 497 }
PicYusuke 0:fb4269aa5fb4 498
PicYusuke 0:fb4269aa5fb4 499
PicYusuke 0:fb4269aa5fb4 500 return 0;
PicYusuke 0:fb4269aa5fb4 501 }
PicYusuke 0:fb4269aa5fb4 502
PicYusuke 0:fb4269aa5fb4 503
PicYusuke 0:fb4269aa5fb4 504
PicYusuke 0:fb4269aa5fb4 505
PicYusuke 0:fb4269aa5fb4 506
PicYusuke 0:fb4269aa5fb4 507
PicYusuke 0:fb4269aa5fb4 508
PicYusuke 0:fb4269aa5fb4 509
PicYusuke 0:fb4269aa5fb4 510
PicYusuke 0:fb4269aa5fb4 511
PicYusuke 0:fb4269aa5fb4 512
PicYusuke 0:fb4269aa5fb4 513
PicYusuke 0:fb4269aa5fb4 514
PicYusuke 0:fb4269aa5fb4 515
PicYusuke 0:fb4269aa5fb4 516
PicYusuke 0:fb4269aa5fb4 517
PicYusuke 0:fb4269aa5fb4 518
PicYusuke 0:fb4269aa5fb4 519
PicYusuke 0:fb4269aa5fb4 520
PicYusuke 0:fb4269aa5fb4 521 #if 0
PicYusuke 0:fb4269aa5fb4 522
PicYusuke 0:fb4269aa5fb4 523 int32_t Task8(void) //棚左側にUFOを置くために、位置合わせ
PicYusuke 0:fb4269aa5fb4 524 {
PicYusuke 0:fb4269aa5fb4 525 uint32_t i;
PicYusuke 0:fb4269aa5fb4 526 float v = 0;
PicYusuke 0:fb4269aa5fb4 527 /*
PicYusuke 0:fb4269aa5fb4 528 if()
PicYusuke 0:fb4269aa5fb4 529 {
PicYusuke 0:fb4269aa5fb4 530 return ERROR;
PicYusuke 0:fb4269aa5fb4 531 }
PicYusuke 0:fb4269aa5fb4 532 */
PicYusuke 0:fb4269aa5fb4 533
PicYusuke 0:fb4269aa5fb4 534 /*-------------------ライントレース--------------------*/
PicYusuke 0:fb4269aa5fb4 535 current_pid_ptr = &trace_pid; //制御器変更
PicYusuke 0:fb4269aa5fb4 536 Accelerate(2.0f); //加速
PicYusuke 0:fb4269aa5fb4 537
PicYusuke 0:fb4269aa5fb4 538 for(i = 0; i < 3; i ++) //1-3回目の交差点はスキップ
PicYusuke 0:fb4269aa5fb4 539 {
PicYusuke 0:fb4269aa5fb4 540 while(!intersection) //一回目の交差点検出はスキップ
PicYusuke 0:fb4269aa5fb4 541 {
PicYusuke 0:fb4269aa5fb4 542 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 543 }
PicYusuke 0:fb4269aa5fb4 544 while(intersection) //一回目の交差点通過を待つ
PicYusuke 0:fb4269aa5fb4 545 {
PicYusuke 0:fb4269aa5fb4 546 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 547 }
PicYusuke 0:fb4269aa5fb4 548 }
PicYusuke 0:fb4269aa5fb4 549 /*-----------------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 550
PicYusuke 0:fb4269aa5fb4 551 /*---------------4回目の交差点-----------------*/
PicYusuke 0:fb4269aa5fb4 552 while(!intersection) //交差点検出まで直進
PicYusuke 0:fb4269aa5fb4 553 {
PicYusuke 0:fb4269aa5fb4 554 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 555 }
PicYusuke 0:fb4269aa5fb4 556 while(intersection) //交差点通過を待つ
PicYusuke 0:fb4269aa5fb4 557 {
PicYusuke 0:fb4269aa5fb4 558 printf("running...\n");
PicYusuke 0:fb4269aa5fb4 559 }
PicYusuke 0:fb4269aa5fb4 560
PicYusuke 0:fb4269aa5fb4 561 wait(1.5f);
PicYusuke 0:fb4269aa5fb4 562 /*--------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 563
PicYusuke 0:fb4269aa5fb4 564 Stop();
PicYusuke 0:fb4269aa5fb4 565
PicYusuke 0:fb4269aa5fb4 566 return 0;
PicYusuke 0:fb4269aa5fb4 567 }
PicYusuke 0:fb4269aa5fb4 568
PicYusuke 0:fb4269aa5fb4 569 int32_t Task9(void)
PicYusuke 0:fb4269aa5fb4 570 {
PicYusuke 0:fb4269aa5fb4 571 uint32_t i;
PicYusuke 0:fb4269aa5fb4 572 float v = 0;
PicYusuke 0:fb4269aa5fb4 573 /*
PicYusuke 0:fb4269aa5fb4 574 if()
PicYusuke 0:fb4269aa5fb4 575 {
PicYusuke 0:fb4269aa5fb4 576 return ERROR;
PicYusuke 0:fb4269aa5fb4 577 }
PicYusuke 0:fb4269aa5fb4 578 */
PicYusuke 0:fb4269aa5fb4 579
PicYusuke 0:fb4269aa5fb4 580 while(1)
PicYusuke 0:fb4269aa5fb4 581 {
PicYusuke 0:fb4269aa5fb4 582 printf("Tasks completed\n");
PicYusuke 0:fb4269aa5fb4 583 }
PicYusuke 0:fb4269aa5fb4 584
PicYusuke 0:fb4269aa5fb4 585
PicYusuke 0:fb4269aa5fb4 586 return 0;
PicYusuke 0:fb4269aa5fb4 587 }
PicYusuke 0:fb4269aa5fb4 588
PicYusuke 0:fb4269aa5fb4 589 #endif
PicYusuke 0:fb4269aa5fb4 590
PicYusuke 0:fb4269aa5fb4 591 void Turn_90deg(int32_t dir)
PicYusuke 0:fb4269aa5fb4 592 {
PicYusuke 0:fb4269aa5fb4 593 uint32_t i;
PicYusuke 0:fb4269aa5fb4 594 float v = 0.0f;
PicYusuke 0:fb4269aa5fb4 595 float sign = 1.0f;
PicYusuke 0:fb4269aa5fb4 596
PicYusuke 0:fb4269aa5fb4 597 if(dir == 1) //回転方向
PicYusuke 0:fb4269aa5fb4 598 {
PicYusuke 0:fb4269aa5fb4 599 sign = 1.0f;
PicYusuke 0:fb4269aa5fb4 600 }
PicYusuke 0:fb4269aa5fb4 601 else if(dir == -1)
PicYusuke 0:fb4269aa5fb4 602 {
PicYusuke 0:fb4269aa5fb4 603 sign = -1.0f;
PicYusuke 0:fb4269aa5fb4 604 }
PicYusuke 0:fb4269aa5fb4 605
PicYusuke 0:fb4269aa5fb4 606 theta = 0.0f; //角度初期化
PicYusuke 0:fb4269aa5fb4 607
PicYusuke 0:fb4269aa5fb4 608 /*--------台形加速(角度)---------*/
PicYusuke 0:fb4269aa5fb4 609 for(i = 0; i < 10; i ++)
PicYusuke 0:fb4269aa5fb4 610 {
PicYusuke 0:fb4269aa5fb4 611 v += 0.15f;
PicYusuke 0:fb4269aa5fb4 612 Put_Vm_L(-sign * v); //回転
PicYusuke 0:fb4269aa5fb4 613 Put_Vm_R(sign * v);
PicYusuke 0:fb4269aa5fb4 614 wait(0.05f);
PicYusuke 0:fb4269aa5fb4 615 }
PicYusuke 0:fb4269aa5fb4 616 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 617
PicYusuke 0:fb4269aa5fb4 618 if(dir == 1)
PicYusuke 0:fb4269aa5fb4 619 {
PicYusuke 0:fb4269aa5fb4 620 while(theta < 80.0f)
PicYusuke 0:fb4269aa5fb4 621 {
PicYusuke 0:fb4269aa5fb4 622 printf("Turning...\n");
PicYusuke 0:fb4269aa5fb4 623 }
PicYusuke 0:fb4269aa5fb4 624 }
PicYusuke 0:fb4269aa5fb4 625 else if(dir == -1)
PicYusuke 0:fb4269aa5fb4 626 {
PicYusuke 0:fb4269aa5fb4 627 while(theta > -80.0f)
PicYusuke 0:fb4269aa5fb4 628 {
PicYusuke 0:fb4269aa5fb4 629 printf("Turning...\n");
PicYusuke 0:fb4269aa5fb4 630 }
PicYusuke 0:fb4269aa5fb4 631 }
PicYusuke 0:fb4269aa5fb4 632
PicYusuke 0:fb4269aa5fb4 633 Put_Vm_L(0.0f); //停止
PicYusuke 0:fb4269aa5fb4 634 Put_Vm_R(0.0f);
PicYusuke 0:fb4269aa5fb4 635 wait(0.5f);
PicYusuke 0:fb4269aa5fb4 636 }
PicYusuke 0:fb4269aa5fb4 637
PicYusuke 0:fb4269aa5fb4 638 void Turn_180deg(int32_t dir)
PicYusuke 0:fb4269aa5fb4 639 {
PicYusuke 0:fb4269aa5fb4 640 uint32_t i;
PicYusuke 0:fb4269aa5fb4 641 float v = 0.0f;
PicYusuke 0:fb4269aa5fb4 642 float sign = 1.0f;
PicYusuke 0:fb4269aa5fb4 643
PicYusuke 0:fb4269aa5fb4 644 if(dir == 1) //回転方向
PicYusuke 0:fb4269aa5fb4 645 {
PicYusuke 0:fb4269aa5fb4 646 sign = 1.0f;
PicYusuke 0:fb4269aa5fb4 647 }
PicYusuke 0:fb4269aa5fb4 648 else if(dir == -1)
PicYusuke 0:fb4269aa5fb4 649 {
PicYusuke 0:fb4269aa5fb4 650 sign = -1.0f;
PicYusuke 0:fb4269aa5fb4 651 }
PicYusuke 0:fb4269aa5fb4 652
PicYusuke 0:fb4269aa5fb4 653 theta = 0.0f; //角度初期化
PicYusuke 0:fb4269aa5fb4 654
PicYusuke 0:fb4269aa5fb4 655 /*--------台形加速(角度)---------*/
PicYusuke 0:fb4269aa5fb4 656 for(i = 0; i < 10; i ++)
PicYusuke 0:fb4269aa5fb4 657 {
PicYusuke 0:fb4269aa5fb4 658 v += 0.15f;
PicYusuke 0:fb4269aa5fb4 659 Put_Vm_L(-sign * v); //回転
PicYusuke 0:fb4269aa5fb4 660 Put_Vm_R(sign * v);
PicYusuke 0:fb4269aa5fb4 661 wait(0.05f);
PicYusuke 0:fb4269aa5fb4 662 }
PicYusuke 0:fb4269aa5fb4 663 /*-----------------------------*/
PicYusuke 0:fb4269aa5fb4 664
PicYusuke 0:fb4269aa5fb4 665 while(theta < 170.0f)
PicYusuke 0:fb4269aa5fb4 666 {
PicYusuke 0:fb4269aa5fb4 667 printf("Turning...\n");
PicYusuke 0:fb4269aa5fb4 668 }
PicYusuke 0:fb4269aa5fb4 669 Put_Vm_L(0.0f); //停止
PicYusuke 0:fb4269aa5fb4 670 Put_Vm_R(0.0f);
PicYusuke 0:fb4269aa5fb4 671 wait(0.5f);
PicYusuke 0:fb4269aa5fb4 672 }
PicYusuke 0:fb4269aa5fb4 673
PicYusuke 0:fb4269aa5fb4 674 void Accelerate(float v_desired) //v_desired[V]まで緩やかに加速
PicYusuke 0:fb4269aa5fb4 675 {
PicYusuke 0:fb4269aa5fb4 676 uint32_t i;
PicYusuke 0:fb4269aa5fb4 677 float v = 0.0f;
PicYusuke 0:fb4269aa5fb4 678 float v_step = v_desired / 10.0f; //単位速度
PicYusuke 0:fb4269aa5fb4 679
PicYusuke 0:fb4269aa5fb4 680 for(i = 0; i < 10; i ++)
PicYusuke 0:fb4269aa5fb4 681 {
PicYusuke 0:fb4269aa5fb4 682 v += v_step;
PicYusuke 0:fb4269aa5fb4 683 v_tr = v;
PicYusuke 0:fb4269aa5fb4 684 wait(0.05f);
PicYusuke 0:fb4269aa5fb4 685 }
PicYusuke 0:fb4269aa5fb4 686 }
PicYusuke 0:fb4269aa5fb4 687
PicYusuke 0:fb4269aa5fb4 688 void Decelerate() //緩やかに減速、停止
PicYusuke 0:fb4269aa5fb4 689 {
PicYusuke 0:fb4269aa5fb4 690 uint32_t i;
PicYusuke 0:fb4269aa5fb4 691 float v = 0.0f;
PicYusuke 0:fb4269aa5fb4 692 float v_step = -v_tr / 10.0f;
PicYusuke 0:fb4269aa5fb4 693
PicYusuke 0:fb4269aa5fb4 694 for(i = 0; i < 10; i ++)
PicYusuke 0:fb4269aa5fb4 695 {
PicYusuke 0:fb4269aa5fb4 696 v += v_step;
PicYusuke 0:fb4269aa5fb4 697 v_tr = v;
PicYusuke 0:fb4269aa5fb4 698 wait(0.05f);
PicYusuke 0:fb4269aa5fb4 699 }
PicYusuke 0:fb4269aa5fb4 700
PicYusuke 0:fb4269aa5fb4 701 v_tr = 0.0f; //確実に停止
PicYusuke 0:fb4269aa5fb4 702 wait(0.1f);
PicYusuke 0:fb4269aa5fb4 703 }
PicYusuke 0:fb4269aa5fb4 704
PicYusuke 0:fb4269aa5fb4 705 void Stop() //急激に減速、停止
PicYusuke 0:fb4269aa5fb4 706 {
PicYusuke 0:fb4269aa5fb4 707 current_pid_ptr = &manual_control; //制御OFF
PicYusuke 0:fb4269aa5fb4 708 Put_Vm_L(0.0f);
PicYusuke 0:fb4269aa5fb4 709 Put_Vm_R(0.0f);
PicYusuke 0:fb4269aa5fb4 710 wait(0.5f);
PicYusuke 0:fb4269aa5fb4 711 }
PicYusuke 0:fb4269aa5fb4 712
PicYusuke 0:fb4269aa5fb4 713 void Flip1_Callback() //RCサーボ制御用割り込み関数
PicYusuke 0:fb4269aa5fb4 714 {
PicYusuke 0:fb4269aa5fb4 715 switch(servo_state)
PicYusuke 0:fb4269aa5fb4 716 {
PicYusuke 0:fb4269aa5fb4 717 case 0:
PicYusuke 0:fb4269aa5fb4 718 flipper1.attach(&Flip1_Callback, servo_pulse);
PicYusuke 0:fb4269aa5fb4 719 servo = 1;
PicYusuke 0:fb4269aa5fb4 720 servo_state ++;
PicYusuke 0:fb4269aa5fb4 721 break;
PicYusuke 0:fb4269aa5fb4 722 case 1:
PicYusuke 0:fb4269aa5fb4 723 flipper1.attach(&Flip1_Callback, 0.01f - servo_pulse);
PicYusuke 0:fb4269aa5fb4 724 servo = 0;
PicYusuke 0:fb4269aa5fb4 725 servo_state ++;
PicYusuke 0:fb4269aa5fb4 726 break;
PicYusuke 0:fb4269aa5fb4 727 default:
PicYusuke 0:fb4269aa5fb4 728 servo_state = 0;
PicYusuke 0:fb4269aa5fb4 729 break;
PicYusuke 0:fb4269aa5fb4 730 }
PicYusuke 0:fb4269aa5fb4 731 }
PicYusuke 0:fb4269aa5fb4 732
PicYusuke 0:fb4269aa5fb4 733 void Flip2_Callback() //フィードバック制御用割り込み関数
PicYusuke 0:fb4269aa5fb4 734 {
PicYusuke 0:fb4269aa5fb4 735 int32_t i;
PicYusuke 0:fb4269aa5fb4 736 uint32_t adres_max = 0;
PicYusuke 0:fb4269aa5fb4 737 uint32_t adres_min = 65535;
PicYusuke 0:fb4269aa5fb4 738 uint32_t adres_mid = 0;
PicYusuke 0:fb4269aa5fb4 739 int32_t pos_int = 0;
PicYusuke 0:fb4269aa5fb4 740
PicYusuke 0:fb4269aa5fb4 741 float omega = 0.0f; //unit: deg/s
PicYusuke 0:fb4269aa5fb4 742 float vm_L, vm_R; //unit: V
PicYusuke 0:fb4269aa5fb4 743
PicYusuke 0:fb4269aa5fb4 744 /*-----------------センサ読み取り-----------------*/
PicYusuke 0:fb4269aa5fb4 745 //MCP3208_Read(0);
PicYusuke 0:fb4269aa5fb4 746 MCP3208_Read(1);
PicYusuke 0:fb4269aa5fb4 747 MCP3208_Read(2);
PicYusuke 0:fb4269aa5fb4 748 MCP3208_Read(3);
PicYusuke 0:fb4269aa5fb4 749 MCP3208_Read(4);
PicYusuke 0:fb4269aa5fb4 750 MCP3208_Read(5);
PicYusuke 0:fb4269aa5fb4 751
PicYusuke 0:fb4269aa5fb4 752 L3GD20_Read();
PicYusuke 0:fb4269aa5fb4 753 /*-----------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 754
PicYusuke 0:fb4269aa5fb4 755 /*---------------最大最小値を求める----------------*/
PicYusuke 0:fb4269aa5fb4 756 for(i = 0; i < 5; i ++)
PicYusuke 0:fb4269aa5fb4 757 {
PicYusuke 0:fb4269aa5fb4 758 mcp3208.adres[i+1] = 4095 - mcp3208.adres[i+1]; //ADCの出力とラインの色に合わせて反転/非反転
PicYusuke 0:fb4269aa5fb4 759
PicYusuke 0:fb4269aa5fb4 760 if(adres_min > mcp3208.adres[i+1])
PicYusuke 0:fb4269aa5fb4 761 adres_min = mcp3208.adres[i+1];
PicYusuke 0:fb4269aa5fb4 762 if(adres_max < mcp3208.adres[i+1])
PicYusuke 0:fb4269aa5fb4 763 adres_max = mcp3208.adres[i+1];
PicYusuke 0:fb4269aa5fb4 764 }
PicYusuke 0:fb4269aa5fb4 765
PicYusuke 0:fb4269aa5fb4 766 adres_mid = (adres_max + adres_min) / 2;
PicYusuke 0:fb4269aa5fb4 767 /*-----------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 768 if(adres_max - adres_min > 200)
PicYusuke 0:fb4269aa5fb4 769 {
PicYusuke 0:fb4269aa5fb4 770 line_pos[0] = 0.0f;
PicYusuke 0:fb4269aa5fb4 771 for(i = 0; i < 5; i ++)
PicYusuke 0:fb4269aa5fb4 772 {
PicYusuke 0:fb4269aa5fb4 773 pos_int += (i+1)*(mcp3208.adres[i+1] - adres_min);
PicYusuke 0:fb4269aa5fb4 774 }
PicYusuke 0:fb4269aa5fb4 775 line_pos[0] = ((float)pos_int);
PicYusuke 0:fb4269aa5fb4 776 line_pos[0] = line_pos[0] / (mcp3208.adres[1]+mcp3208.adres[2]+mcp3208.adres[3]+mcp3208.adres[4]+mcp3208.adres[5]-5*adres_min) - 3.0f;
PicYusuke 0:fb4269aa5fb4 777 }
PicYusuke 0:fb4269aa5fb4 778 else
PicYusuke 0:fb4269aa5fb4 779 {
PicYusuke 0:fb4269aa5fb4 780 line_pos[0] = 0.0f;
PicYusuke 0:fb4269aa5fb4 781 }
PicYusuke 0:fb4269aa5fb4 782
PicYusuke 0:fb4269aa5fb4 783 /*-------------------交差点検出-------------------*/
PicYusuke 0:fb4269aa5fb4 784 if((mcp3208.adres[1] > adres_mid) && (mcp3208.adres[5] > adres_mid))
PicYusuke 0:fb4269aa5fb4 785 {
PicYusuke 0:fb4269aa5fb4 786 intersection = 1;
PicYusuke 0:fb4269aa5fb4 787 }
PicYusuke 0:fb4269aa5fb4 788 else if((mcp3208.adres[1] < adres_mid) && (mcp3208.adres[5] < adres_mid))
PicYusuke 0:fb4269aa5fb4 789 {
PicYusuke 0:fb4269aa5fb4 790 intersection = 0;
PicYusuke 0:fb4269aa5fb4 791 }
PicYusuke 0:fb4269aa5fb4 792 /*-----------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 793
PicYusuke 0:fb4269aa5fb4 794 /*---------------フィルタ----------------*/
PicYusuke 0:fb4269aa5fb4 795
PicYusuke 0:fb4269aa5fb4 796 /*--------------------------------------*/
PicYusuke 0:fb4269aa5fb4 797
PicYusuke 0:fb4269aa5fb4 798 /*-----------ジャイロ角速度取得-----------*/
PicYusuke 0:fb4269aa5fb4 799 omega = (float)l3gd20.z * 0.07f;
PicYusuke 0:fb4269aa5fb4 800 theta += omega * ts;
PicYusuke 0:fb4269aa5fb4 801 /*--------------------------------------*/
PicYusuke 0:fb4269aa5fb4 802
PicYusuke 0:fb4269aa5fb4 803 /*------------------制御則-----------------*/
PicYusuke 0:fb4269aa5fb4 804 if(current_pid_ptr == &trace_pid) //ライントレース
PicYusuke 0:fb4269aa5fb4 805 {
PicYusuke 0:fb4269aa5fb4 806 //vm_R = PD_Control(current_pid_ptr, omega, 10.0f * line_pos[0]);
PicYusuke 0:fb4269aa5fb4 807 vm_R = PD_Control(current_pid_ptr, -line_pos[0], 0.0f);
PicYusuke 0:fb4269aa5fb4 808 vm_L = -vm_R;
PicYusuke 0:fb4269aa5fb4 809
PicYusuke 0:fb4269aa5fb4 810 vm_L += v_tr;
PicYusuke 0:fb4269aa5fb4 811 vm_R += v_tr;
PicYusuke 0:fb4269aa5fb4 812 }
PicYusuke 0:fb4269aa5fb4 813 else if(current_pid_ptr == &gyro_pid)
PicYusuke 0:fb4269aa5fb4 814 {
PicYusuke 0:fb4269aa5fb4 815 vm_R = PD_Control(current_pid_ptr, omega, omega_d);
PicYusuke 0:fb4269aa5fb4 816 vm_L = -vm_R;
PicYusuke 0:fb4269aa5fb4 817
PicYusuke 0:fb4269aa5fb4 818 vm_L += v_tr;
PicYusuke 0:fb4269aa5fb4 819 vm_R += v_tr;
PicYusuke 0:fb4269aa5fb4 820 }
PicYusuke 0:fb4269aa5fb4 821 else
PicYusuke 0:fb4269aa5fb4 822 {
PicYusuke 0:fb4269aa5fb4 823 vm_L = 0.0f;
PicYusuke 0:fb4269aa5fb4 824 vm_R = 0.0f;
PicYusuke 0:fb4269aa5fb4 825 }
PicYusuke 0:fb4269aa5fb4 826
PicYusuke 0:fb4269aa5fb4 827 if(current_pid_ptr == &manual_control)
PicYusuke 0:fb4269aa5fb4 828 {
PicYusuke 0:fb4269aa5fb4 829 //手動でモータ電圧を管理している場合
PicYusuke 0:fb4269aa5fb4 830 }
PicYusuke 0:fb4269aa5fb4 831 else
PicYusuke 0:fb4269aa5fb4 832 {
PicYusuke 0:fb4269aa5fb4 833 Put_Vm_L(vm_L); //モータへ電圧印加
PicYusuke 0:fb4269aa5fb4 834 Put_Vm_R(vm_R);
PicYusuke 0:fb4269aa5fb4 835 }
PicYusuke 0:fb4269aa5fb4 836
PicYusuke 0:fb4269aa5fb4 837 /*----------------------------------------*/
PicYusuke 0:fb4269aa5fb4 838 }
PicYusuke 0:fb4269aa5fb4 839
PicYusuke 0:fb4269aa5fb4 840 void Flip3_Callback()
PicYusuke 0:fb4269aa5fb4 841 {
PicYusuke 0:fb4269aa5fb4 842
PicYusuke 0:fb4269aa5fb4 843
PicYusuke 0:fb4269aa5fb4 844
PicYusuke 0:fb4269aa5fb4 845 }
PicYusuke 0:fb4269aa5fb4 846
PicYusuke 0:fb4269aa5fb4 847 void PinIntrrpt1_Callback()
PicYusuke 0:fb4269aa5fb4 848 {
PicYusuke 0:fb4269aa5fb4 849
PicYusuke 0:fb4269aa5fb4 850 }
PicYusuke 0:fb4269aa5fb4 851
PicYusuke 0:fb4269aa5fb4 852 void PinIntrrpt2_Callback()
PicYusuke 0:fb4269aa5fb4 853 {
PicYusuke 0:fb4269aa5fb4 854
PicYusuke 0:fb4269aa5fb4 855 }