Yuhi Takaku
ウオッチドッグタイマを追加したよん
main.cpp
- Committer:
- yuron
- Date:
- 2019-11-02
- Revision:
- 30:2aa3d5dd117d
- Parent:
- 29:81b1ec07b5c2
File content as of revision 30:2aa3d5dd117d:
/* ------------------------------------------------------------------- */
/* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */
/* Nucleo Type: F446RE */
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
/* Actuator: RS-555*4, RS-385*4, RZ-735*2, PWM_Servo(KONDO)*2 */
/* Sensor: encorder*4, limit_switch*14 */
/* ------------------------------------------------------------------- */
/* Both of areas are compleated! */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
//終了phase
#define FINAL_PHASE 50
#define RED 0
#define BLUE 1
//#define wind_time1 1.00f
//#define wind_time2 1.00f
#define wind_time1 0.75f
//#define wind_time2 1.50f
#define wind_time2 1.75f
//PID Gain of wheels(Kp, Ti, Td, control cycle)
//前進
PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//後進
PID back_migimae(4500000.0, 0.0, 0.0, 0.001);
PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//右進
PID right_migimae(6000000.0, 0.0, 0.0, 0.001);
PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001);
PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001);
PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
//左進
PID left_migimae(6000000.0, 0.0, 0.0, 0.001);
PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001);
PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001);
PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
//右旋回
PID turn_right_migimae(3000000.0, 0.0, 0.0, 0.001);
PID turn_right_migiusiro(3000000.0, 0.0, 0.0, 0.001);
PID turn_right_hidarimae(3000000.0, 0.0, 0.0, 0.001);
PID turn_right_hidariusiro(3000000.0, 0.0, 0.0, 0.001);
//左旋回
PID turn_left_migimae(3000000.0, 0.0, 0.0, 0.001);
PID turn_left_migiusiro(3000000.0, 0.0, 0.0, 0.001);
PID turn_left_hidarimae(3000000.0, 0.0, 0.0, 0.001);
PID turn_left_hidariusiro(3000000.0, 0.0, 0.0, 0.001);
//MDとの通信ポート
I2C i2c(PB_9, PB_8); //SDA, SCL
//PCとの通信ポート
Serial pc(USBTX, USBRX); //TX, RX
//特小モジュールとの通信ポート
Serial pic(A0, A1);
//リミットスイッチ基板との通信ポート
Serial limit_serial(PC_12, PD_2);
//12V停止信号ピン
DigitalOut emergency(D11);
DigitalOut USR_LED1(PB_7);
//DigitalOut USR_LED2(PC_13);
DigitalOut USR_LED3(PC_2);
DigitalOut USR_LED4(PC_3);
DigitalOut GREEN_LED(D8);
DigitalOut RED_LED(D10);
DigitalOut YELLOW_LED(D9);
//遠隔非常停止ユニットLED
AnalogOut myled(A2);
DigitalIn start_switch(PB_12);
DigitalIn USR_SWITCH(PC_13);
DigitalIn zone_switch(PC_10);
QEI wheel_x1(PA_8 , PA_6 , NC, 624);
QEI wheel_x2(PB_14, PB_13, NC, 624);
QEI wheel_y1(PB_1 , PB_15, NC, 624);
QEI wheel_y2(PA_12, PA_11, NC, 624);
QEI arm_enc(PB_4, PB_5 , NC, 624);
//移動後n秒停止タイマー
Timer counter;
//Watch Dog Timer
Timer WDT;
//エンコーダ値格納変数
int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse, arm_pulse;
//操作の段階変数
unsigned int phase = 0;
unsigned int kaisyu_phase = 0;
unsigned int tyokudo_phase = 0;
unsigned int start_zone = 1;
bool zone = RED;
bool stop_flag[30] = {0};
//i2c送信データ変数
char init_send_data[1];
char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
char arm_motor[1], drop_motor[1];
char fan_data[1];
char servo_data[1];
char right_arm_data[1], left_arm_data[1];
//非常停止関連変数
char RDATA;
char baff;
int flug = 0;
//リミット基板からの受信データ
int limit_data = 0;
int upper_limit_data = 0;
int lower_limit_data = 0;
//各辺のスイッチが押されたかのフラグ
//前部が壁に当たっているか
int front_limit = 0;
//右部が壁にあたあっているか
int right_limit = 0;
//後部が壁に当たっているか
int back_limit = 0;
//回収機構の下限(引っ込めてるほう)
bool kaisyu_mae_limit = 0;
bool kaisyu_usiro_limit = 0;
//右腕の下限
bool right_arm_lower_limit = 0;
//右腕の上限
bool right_arm_upper_limit = 0;
//左腕の下限
bool left_arm_lower_limit = 0;
//左腕の上限
bool left_arm_upper_limit = 0;
//吐き出し機構の上限
bool tyokudo_mae_limit = 0;
//吐き出し機構の下限
bool tyokudo_usiro_limit = 0;
int masked_lower_front_limit_data = 0b11111111;
int masked_lower_back_limit_data = 0b11111111;
int masked_lower_right_limit_data = 0b11111111;
int masked_kaisyu_mae_limit_data = 0b11111111;
int masked_kaisyu_usiro_limit_data = 0b11111111;
int masked_right_arm_lower_limit_data = 0b11111111;
int masked_right_arm_upper_limit_data = 0b11111111;
int masked_left_arm_lower_limit_data = 0b11111111;
int masked_left_arm_upper_limit_data = 0b11111111;
int masked_tyokudo_mae_limit_data = 0b11111111;
int masked_tyokudo_usiro_limit_data = 0b11111111;
//関数のプロトタイプ宣言
void red_move(void);
void blue_move(void);
void init(void);
void init_send(void);
void get(void);
void get_pulses(void);
void print_pulses(void);
void get_emergency(void);
void read_limit(void);
void wheel_reset(void);
void kaisyu(int pulse, int next_phase);
void kaisyu_nobasu(int next_phase);
void kaisyu_hiku(int next_phase);
void tyokudo(int pulse, int next_phase);
void tyokudo_nobasu(int next_phase);
void tyokudo_hiku(int next_phase);
void arm_up(int next_phase);
void fan_on(float first_wind_time, float second_wind_time, int next_phase);
void front(int target);
void back(int target);
void right(int target);
void left(int target);
void turn_right(int target);
void turn_left(int target);
void stop(void);
void all_stop(void);
void front_PID(int target);
void back_PID(int target);
void right_PID(int target);
void left_PID(int target);
void turn_right_PID(int target);
void turn_left_PID(int target);
int main(void) {
init();
init_send();
//消し忘れ注意!!
//phase = 50;
//起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
while(1) {
if(zone_switch == 0) {
zone = BLUE;
} else {
zone = RED;
}
break;
}
while(1) {
get_pulses();
//print_pulses();
get_emergency();
read_limit();
//move_servo_with_using_onboard-switch
if(USR_SWITCH == 0) {
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
} else {
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
}
/*
//消し忘れ注意!!
if(start_switch == 1) {
counter.reset();
if(zone == RED) {
phase = 35;
}
else if(zone == BLUE) {
phase = 38;
}
}
*/
//青ゾーン
if(zone == BLUE) {
GREEN_LED = 1;
RED_LED = 0;
blue_move();
}
//REDゾーン
else if(zone == RED) {
GREEN_LED = 0;
RED_LED = 1;
red_move();
}
}
}
void red_move(void) {
switch(phase) {
//スタート位置へセット
case 0:
//スタートスイッチが押されたか
if(start_switch == 1) {
wheel_reset();
phase = 1;
}
//リミットが洗濯物台に触れているか
if(right_limit == 3) {
USR_LED1 = 1;
} else {
USR_LED1 = 0;
}
break;
//回収アームを伸ばす
case 1:
kaisyu_nobasu(2);
//サーボを開いておく
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
//1.0秒停止
case 2:
if(stop_flag[0] == 0) {
stop();
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
counter.stop();
counter.reset();
counter.start();
stop_flag[0] = 1;
}
if(counter.read() > 1.0f) {
phase = 3;
wheel_reset();
}
break;
//ちょっと前進
case 3:
front(800);
if((y_pulse1 > 800) || (y_pulse2 > 800)) {
phase = 4;
}
break;
//0.5秒停止
case 4:
WDT.start();
if(stop_flag[1] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[1] = 1;
}
if(counter.read() > 0.5f) {
phase = 5;
wheel_reset();
}
//WDTで3秒経過したら強制的にphase5に移行
if(WDT.read() > 3.0f) {
phase = 5;
wheel_reset();
}
break;
//回収アーム引っ込める
case 5:
USR_LED3 = 1;
kaisyu_hiku(6);
break;
//左移動
case 6:
USR_LED4 = 1;
left(11500);
if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
phase = 7;
}
break;
//1秒停止
case 7:
if(stop_flag[2] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[2] = 1;
}
if(counter.read() > 1.0f) {
phase = 8;
wheel_reset();
}
break;
//右旋回(180°)
case 8:
turn_right(940);
if(sum_pulse > 940) {
phase = 9;
}
break;
//0.5秒停止
case 9:
if(stop_flag[3] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[3] = 1;
}
if(counter.read() > 0.5f) {
phase = 10;
wheel_reset();
}
break;
//壁に当たるまで前進
case 10:
if(front_limit == 3) {
phase = 11;
}
else if(front_limit != 3){
true_migimae_data[0] = 0xA0;
true_migiusiro_data[0] = 0xA0;
true_hidarimae_data[0] = 0xA0;
true_hidariusiro_data[0] = 0xA0;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//0.5秒停止
case 11:
if(stop_flag[4] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[4] = 1;
}
if(counter.read() > 0.5f) {
phase = 12;
wheel_reset();
}
break;
//壁に当たるまで右移動
case 12:
if(right_limit == 3) {
phase = 13;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
true_migiusiro_data[0] = 0xBF;
true_hidarimae_data[0] = 0xBF;
true_hidariusiro_data[0] = 0x40;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//0.5秒停止
case 13:
if(stop_flag[5] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[5] = 1;
}
if(counter.read() > 0.5f) {
phase = 14;
wheel_reset();
}
break;
//排出
case 14:
tyokudo_nobasu(15);
break;
//後進
case 15:
back(-5000);
if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
phase = 16;
}
break;
//0.5秒停止
case 16:
if(stop_flag[6] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[6] = 1;
}
if(counter.read() > 0.5f) {
phase = 17;
wheel_reset();
}
break;
//排出しまう
case 17:
tyokudo_hiku(18);
break;
//0.5秒停止
case 18:
if(stop_flag[7] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[7] = 1;
}
if(counter.read() > 0.5f) {
phase = 19;
wheel_reset();
}
break;
//壁に当たるまで右移動
case 19:
if(right_limit == 3) {
phase = 20;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
true_migiusiro_data[0] = 0xBF;
true_hidarimae_data[0] = 0xBF;
true_hidariusiro_data[0] = 0x40;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//0.5秒停止
case 20:
if(stop_flag[8] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[8] = 1;
}
if(counter.read() > 0.5f) {
phase = 21;
wheel_reset();
}
break;
//壁に当たるまで前進
case 21:
if(front_limit == 3) {
phase = 22;
}
else if(front_limit != 3){
true_migimae_data[0] = 0xA0;
true_migiusiro_data[0] = 0xA0;
true_hidarimae_data[0] = 0xA0;
true_hidariusiro_data[0] = 0xA0;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//シーツ装填
case 22:
YELLOW_LED = 1;
if(start_switch == 1) {
wheel_reset();
phase = 23;
} else {
if(stop_flag[9] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[9] = 1;
}
}
break;
//竿のラインまで後進
case 23:
back(-20500);
if((y_pulse1*-1 > 20500) || (y_pulse2*-1 > 20500)) {
phase = 24;
}
break;
//1秒停止
case 24:
if(stop_flag[10] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[10] = 1;
}
if(counter.read() > 1.0f) {
phase = 25;
wheel_reset();
}
break;
//ちょっと左移動
case 25:
left(400);
if((x_pulse1 > 400) || (x_pulse2 > 400)) {
phase = 26;
}
break;
//1秒停止
case 26:
if(stop_flag[11] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[11] = 1;
}
if(counter.read() > 1.0f) {
phase = 27;
wheel_reset();
}
break;
//90°左旋回
case 27:
turn_left(500);
if(sum_pulse > 500) {
phase = 28;
}
break;
//1秒停止
case 28:
if(stop_flag[12] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[12] = 1;
}
if(counter.read() > 1.0f) {
phase = 29;
wheel_reset();
}
break;
//壁に当たるまで後進
case 29:
if(back_limit == 3) {
phase = 30;
}
else if(back_limit != 3){
true_migimae_data[0] = 0x60;
true_migiusiro_data[0] = 0x60;
true_hidarimae_data[0] = 0x60;
true_hidariusiro_data[0] = 0x60;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//1秒停止
case 30:
if(stop_flag[13] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[13] = 1;
}
if(counter.read() > 1.0f) {
phase = 31;
wheel_reset();
}
break;
//掛けるところまで前進
case 31:
front(9200);
if((y_pulse1 > 9200) || (y_pulse2 > 9200)) {
phase = 32;
counter.start();
}
break;
//1秒停止
case 32:
if(stop_flag[14] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[14] = 1;
}
if(counter.read() > 1.0f) {
phase = 33;
wheel_reset();
}
break;
//妨害防止の左旋回
case 33:
turn_left(20);
if(sum_pulse > 20) {
phase = 34;
}
break;
//1秒停止
case 34:
if(stop_flag[15] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[15] = 1;
}
if(counter.read() > 1.0f) {
phase = 35;
wheel_reset();
}
break;
//アームアップ
case 35:
arm_up(36);
if(stop_flag[16] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[16] = 1;
}
if(counter.read() > 2.0f) {
fan_data[0] = 0xFF;
} else {
fan_data[0] = 0x80;
}
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
wait_us(20);
break;
//シーツを掛ける
case 36:
if(stop_flag[17] == 0) {
counter.stop();
counter.reset();
counter.start();
stop_flag[17] = 1;
}
fan_on(wind_time1, wind_time2, FINAL_PHASE);
break;
//終了っ！(守衛さん風)
case FINAL_PHASE:
default:
//駆動系統OFF
all_stop();
break;
}
}
void blue_move(void) {
switch(phase) {
//スタート位置へセット
case 0:
//スタートスイッチが押されたか
if(start_switch == 1) {
wheel_reset();
phase = 1;
}
//リミットが洗濯物台に触れているか
if(right_limit == 3) {
USR_LED1 = 1;
} else {
USR_LED1 = 0;
}
break;
//回収アームを伸ばす
case 1:
kaisyu_nobasu(2);
//サーボを開いておく
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
//1.0秒停止
case 2:
if(stop_flag[0] == 0) {
stop();
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
counter.stop();
counter.reset();
counter.start();
stop_flag[0] = 1;
}
if(counter.read() > 1.0f) {
phase = 3;
wheel_reset();
}
break;
//壁に当たるまで前進
case 3:
/*
if(front_limit == 3) {
phase = 4;
}
else if(front_limit != 3){
true_migimae_data[0] = 0xA0;
true_migiusiro_data[0] = 0xA0;
true_hidarimae_data[0] = 0xA0;
true_hidariusiro_data[0] = 0xA0;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
*/
WDT.start();
front(800);
if((y_pulse1 > 800) || (y_pulse2 > 800)) {
phase = 4;
}
//WDTで5秒経過したら強制的にphase4に移行
if(WDT.read() > 5.0f) {
phase = 4;
wheel_reset();
}
break;
//0.5秒停止
case 4:
if(stop_flag[1] == 0) {
stop();
counter.stop();
WDT.stop();
counter.reset();
WDT.reset();
counter.start();
WDT.start();
stop_flag[1] = 1;
}
if(counter.read() > 0.5f) {
phase = 5;
wheel_reset();
}
//WDTで2秒経過したら強制的にphase5に移行
if(WDT.read() > 2.0f) {
phase = 5;
wheel_reset();
}
break;
//回収アーム引っ込める
case 5:
USR_LED3 = 1;
kaisyu_hiku(6);
break;
//0.5秒停止
case 6:
if(stop_flag[2] == 0) {
USR_LED4 = 1;
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[2] = 1;
}
if(counter.read() > 0.5f) {
phase = 7;
wheel_reset();
}
break;
//ちょっと後進
case 7:
back(-700);
if((y_pulse1*-1 > 700) || (y_pulse2*-1 > 700)) {
phase = 8;
}
break;
//0.5秒停止
case 8:
if(stop_flag[3] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[3] = 1;
}
if(counter.read() > 0.5f) {
phase = 9;
wheel_reset();
}
break;
//左移動
case 9:
left(11500);
if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
phase = 10;
}
break;
//1秒停止
case 10:
if(stop_flag[4] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[4] = 1;
}
if(counter.read() > 1.0f) {
phase = 11;
wheel_reset();
}
break;
//右旋回(180°)
case 11:
counter.reset();
turn_right(950);
if(sum_pulse > 950) {
phase = 12;
}
break;
//0.5秒停止
case 12:
if(stop_flag[5] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[5] = 1;
}
if(counter.read() > 0.5f) {
phase = 13;
wheel_reset();
}
break;
//壁に当たるまで後進
case 13:
if(back_limit == 3) {
phase = 14;
}
else if(back_limit != 3){
true_migimae_data[0] = 0x60;
true_migiusiro_data[0] = 0x60;
true_hidarimae_data[0] = 0x60;
true_hidariusiro_data[0] = 0x60;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//0.5秒停止
case 14:
if(stop_flag[6] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[6] = 1;
}
if(counter.read() > 0.5f) {
phase = 15;
wheel_reset();
}
break;
//壁に当たるまで右移動
case 15:
if(right_limit == 3) {
phase = 16;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
true_migiusiro_data[0] = 0xBF;
true_hidarimae_data[0] = 0xBF;
true_hidariusiro_data[0] = 0x40;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//0.5秒停止
case 16:
if(stop_flag[7] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[7] = 1;
}
if(counter.read() > 0.5f) {
phase = 17;
wheel_reset();
}
break;
//排出
case 17:
tyokudo_nobasu(18);
break;
//前進
case 18:
front(5000);
if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
phase = 19;
}
break;
//0.5秒停止
case 19:
if(stop_flag[8] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[8] = 1;
}
if(counter.read() > 0.5f) {
phase = 20;
wheel_reset();
}
break;
//排出しまう
case 20:
tyokudo_hiku(21);
break;
//0.5秒停止
case 21:
if(stop_flag[9] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[9] = 1;
}
if(counter.read() > 0.5f) {
phase = 22;
wheel_reset();
}
break;
//壁に当たるまで右移動
case 22:
if(right_limit == 3) {
phase = 23;
}
else if(right_limit != 3) {
true_migimae_data[0] = 0x40;
true_migiusiro_data[0] = 0xBF;
true_hidarimae_data[0] = 0xBF;
true_hidariusiro_data[0] = 0x40;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//0.5秒停止
case 23:
if(stop_flag[10] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[10] = 1;
}
if(counter.read() > 0.5f) {
phase = 24;
wheel_reset();
}
break;
//壁に当たるまで後進
case 24:
if(back_limit == 3) {
phase = 25;
}
else if(back_limit != 3){
true_migimae_data[0] = 0x60;
true_migiusiro_data[0] = 0x60;
true_hidarimae_data[0] = 0x60;
true_hidariusiro_data[0] = 0x60;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//シーツ装填
case 25:
YELLOW_LED = 1;
if(start_switch == 1) {
wheel_reset();
phase = 26;
} else {
if(stop_flag[11] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[11] = 1;
}
}
break;
//竿のラインまで前進
case 26:
front(21200);
if((y_pulse1 > 21200) || (y_pulse2 > 21200)) {
phase = 27;
}
break;
//1秒停止
case 27:
if(stop_flag[12] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[12] = 1;
}
if(counter.read() > 1.0f) {
phase = 28;
wheel_reset();
}
break;
//ちょっと左移動
case 28:
left(400);
if((x_pulse1 > 400) || (x_pulse2 > 400)) {
phase = 29;
}
break;
//1秒停止
case 29:
if(stop_flag[13] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[13] = 1;
}
if(counter.read() > 1.0f) {
phase = 30;
wheel_reset();
}
break;
//90°右旋回
case 30:
turn_right(480);
if(sum_pulse > 480) {
phase = 31;
}
break;
//1秒停止
case 31:
if(stop_flag[14] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[14] = 1;
}
if(counter.read() > 1.0f) {
phase = 32;
wheel_reset();
}
break;
//壁に当たるまで前進
case 32:
if(front_limit == 3) {
phase = 33;
}
else if(front_limit != 3){
true_migimae_data[0] = 0xA0;
true_migiusiro_data[0] = 0xA0;
true_hidarimae_data[0] = 0xA0;
true_hidariusiro_data[0] = 0xA0;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
break;
//1秒停止
case 33:
if(stop_flag[15] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[15] = 1;
}
if(counter.read() > 1.0f) {
phase = 34;
wheel_reset();
}
break;
//掛けるところまで後進
case 34:
back(-9200);
if((y_pulse1*-1 > 9200) || (y_pulse2*-1 > 9200)) {
phase = 35;
counter.start();
}
break;
//1秒停止
case 35:
if(stop_flag[16] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[16] = 1;
}
if(counter.read() > 1.0f) {
phase = 36;
wheel_reset();
}
break;
//妨害防止の右旋回
case 36:
turn_right(20);
if(sum_pulse > 20) {
phase = 37;
}
break;
//1秒停止
case 37:
if(stop_flag[17] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[17] = 1;
}
if(counter.read() > 1.0f) {
phase = 38;
wheel_reset();
}
break;
//アームアップ
case 38:
arm_up(39);
if(stop_flag[18] == 0) {
stop();
counter.stop();
counter.reset();
counter.start();
stop_flag[18] = 1;
}
if(counter.read() > 2.0f) {
fan_data[0] = 0xFF;
} else {
fan_data[0] = 0x80;
}
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
wait_us(20);
break;
//シーツを掛ける
case 39:
if(stop_flag[19] == 0) {
counter.stop();
counter.reset();
counter.start();
stop_flag[19] = 1;
}
fan_on(wind_time1, wind_time2, FINAL_PHASE);
break;
//終了っ！(守衛さん風)
case FINAL_PHASE:
default:
//駆動系統OFF
all_stop();
break;
}
}
void init(void) {
//通信ボーレートの設定
pc.baud(460800);
limit_serial.baud(115200);
start_switch.mode(PullUp);
zone_switch.mode(PullDown);
YELLOW_LED = 0;
USR_LED3 = 0; USR_LED4 = 0;
//非常停止関連
pic.baud(19200);
pic.format(8, Serial::None, 1);
pic.attach(get, Serial::RxIrq);
x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; sum_pulse = 0;
migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80;
true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80;
fan_data[0] = 0x80;
servo_data[0] = 0x80;
arm_motor[0] = 0x80; drop_motor[0] = 0x80;
right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
}
void init_send(void) {
init_send_data[0] = 0x80;
i2c.write(0x10, init_send_data, 1);
i2c.write(0x12, init_send_data, 1);
i2c.write(0x14, init_send_data, 1);
i2c.write(0x16, init_send_data, 1);
i2c.write(0x18, init_send_data, 1);
i2c.write(0x20, init_send_data, 1);
i2c.write(0x22, init_send_data, 1);
i2c.write(0x24, init_send_data, 1);
i2c.write(0x30, init_send_data, 1);
wait(0.1);
}
void get(void) {
baff = pic.getc();
for(; flug; flug--)
RDATA = baff;
if(baff == ':')
flug = 1;
}
void get_pulses(void) {
x_pulse1 = wheel_x1.getPulses();
x_pulse2 = wheel_x2.getPulses();
y_pulse1 = wheel_y1.getPulses();
y_pulse2 = wheel_y2.getPulses();
sum_pulse = (abs(x_pulse1) + abs(x_pulse2) + abs(y_pulse1) + abs(y_pulse2)) / 4;
arm_pulse = arm_enc.getPulses();
}
void print_pulses(void) {
//pc.printf("p: %d, kp: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
//pc.printf("p: %d, k_p: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
//pc.printf("X1: %d, X2: %d, Y1: %d, Y2: %d, sum: %d\n\r", abs(x_pulse1), x_pulse2, abs(y_pulse1), y_pulse2, sum_pulse);
//pc.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", front_limit, back_limit, right_limit, kaisyu_mae_limit, kaisyu_usiro_limit,
//tyokudo_mae_limit, tyokudo_usiro_limit, right_arm_upper_limit, left_arm_upper_limit);
//pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]);
//pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
//pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
//pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0], phase);
}
void get_emergency(void) {
if(RDATA == '1') {
myled = 1;
emergency = 1;
}
else if(RDATA == '9'){
myled = 0.2;
emergency = 0;
/*
//終了phaseで駆動系統OFF
if(phase == FINAL_PHASE) {
emergency = 1;
} else {
emergency = 0;
}
*/
}
}
void read_limit(void) {
limit_data = limit_serial.getc();
//上位1bitが1ならば下のリミットのデータだと判断
if((limit_data & 0b10000000) == 0b10000000) {
lower_limit_data = limit_data;
//上位1bitが0ならば上のリミットのデータだと判断
} else {
upper_limit_data = limit_data;
}
//下リミット基板からのデータのマスク処理
masked_lower_front_limit_data = lower_limit_data & 0b00000011;
masked_lower_back_limit_data = lower_limit_data & 0b00001100;
masked_lower_right_limit_data = lower_limit_data & 0b00110000;
masked_kaisyu_mae_limit_data = lower_limit_data & 0b01000000;
//上リミット基板からのデータのマスク処理
//masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001;
masked_kaisyu_usiro_limit_data = upper_limit_data & 0b00000001;
masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010;
masked_left_arm_lower_limit_data = upper_limit_data & 0b00000100;
masked_left_arm_upper_limit_data = upper_limit_data & 0b00001000;
masked_tyokudo_mae_limit_data = upper_limit_data & 0b00010000;
masked_tyokudo_usiro_limit_data = upper_limit_data & 0b00100000;
//前部リミット
switch(masked_lower_front_limit_data) {
//両方押された
case 0x00:
front_limit = 3;
break;
//右が押された
case 0b00000010:
front_limit = 1;
break;
//左が押された
case 0b00000001:
front_limit = 2;
break;
default:
front_limit = 0;
break;
}
//後部リミット
switch(masked_lower_back_limit_data) {
//両方押された
case 0x00:
back_limit = 3;
break;
//右が押された
case 0b00001000:
back_limit = 1;
break;
//左が押された
case 0b00000100:
back_limit = 2;
break;
default:
back_limit = 0;
break;
}
//右部リミット
switch(masked_lower_right_limit_data) {
//両方押された
case 0x00:
right_limit = 3;
break;
//右が押された
case 0b00100000:
right_limit = 1;
break;
//左が押された
case 0b00010000:
right_limit = 2;
break;
default:
right_limit = 0;
break;
}
//回収機構リミット
switch(masked_kaisyu_mae_limit_data) {
//押された
case 0b00000000:
kaisyu_mae_limit = 1;
break;
case 0b01000000:
kaisyu_mae_limit = 0;
break;
default:
kaisyu_mae_limit = 0;
break;
}
//右腕下部リミット
/*
switch(masked_right_arm_lower_limit_data) {
//押された
case 0b00000000:
right_arm_lower_limit = 1;
break;
case 0b00000001:
right_arm_lower_limit = 0;
break;
default:
right_arm_lower_limit = 0;
break;
}
*/
//回収後リミット
switch(masked_kaisyu_usiro_limit_data) {
case 0b00000000:
kaisyu_usiro_limit = 1;
break;
case 0b00000001:
kaisyu_usiro_limit = 0;
break;
default:
kaisyu_usiro_limit = 0;
break;
}
//右腕上部リミット
switch(masked_right_arm_upper_limit_data) {
//押された
case 0b00000000:
right_arm_upper_limit = 1;
break;
case 0b00000010:
right_arm_upper_limit = 0;
break;
default:
right_arm_upper_limit = 0;
break;
}
//左腕下部リミット
switch(masked_left_arm_lower_limit_data) {
//押された
case 0b00000000:
left_arm_lower_limit = 1;
break;
case 0b00000100:
left_arm_lower_limit = 0;
break;
default:
left_arm_lower_limit = 0;
break;
}
//左腕上部リミット
switch(masked_left_arm_upper_limit_data) {
//押された
case 0b00000000:
left_arm_upper_limit = 1;
break;
case 0b00001000:
left_arm_upper_limit = 0;
break;
default:
left_arm_upper_limit = 0;
break;
}
//直動の前
switch(masked_tyokudo_mae_limit_data) {
//押された
case 0b00000000:
tyokudo_mae_limit = 1;
break;
case 0b00010000:
tyokudo_mae_limit = 0;
break;
default:
tyokudo_mae_limit = 0;
break;
}
//直動の後
switch(masked_tyokudo_usiro_limit_data) {
//押された
case 0b00000000:
tyokudo_usiro_limit = 1;
break;
case 0b00100000:
tyokudo_usiro_limit = 0;
break;
default:
tyokudo_usiro_limit = 0;
break;
}
}
void wheel_reset(void) {
wheel_x1.reset();
wheel_x2.reset();
wheel_y1.reset();
wheel_y2.reset();
}
void kaisyu(int pulse, int next_phase) {
switch (kaisyu_phase) {
case 0:
//前進->減速
//3000pulseまで高速前進
if(pulse < 3000) {
arm_motor[0] = 0xFF;
//kaisyu_phase = 1;
}
//3000pulse超えたら低速前進
else if(pulse >= 3000) {
arm_motor[0] = 0xB3;
kaisyu_phase = 1;
}
break;
case 1:
//前進->停止->後進
//3600pulseまで低速前進
if(pulse < 3600) {
arm_motor[0] = 0xB3;
//kaisyu_phase = 2;
}
//3600pulse超えたら停止
else if(pulse >= 3600) {
arm_motor[0] = 0x80;
//1秒待ってから引っ込める
counter.start();
if(counter.read() > 1.0f) {
kaisyu_phase = 2;
}
}
//後ろのリミットが押されたら強制停止
if(kaisyu_usiro_limit == 1) {
arm_motor[0] = 0x80;
//1秒待ってから引っ込める
counter.start();
if(counter.read() > 1.0f) {
kaisyu_phase = 2;
}
}
break;
case 2:
//後進->減速
//500pulseまで高速後進
counter.reset();
if(pulse > 500) {
arm_motor[0] = 0x00;
//kaisyu_phase = 3;
}
//500pulse以下になったら低速後進
else if(pulse <= 500) {
arm_motor[0] = 0x4C;
kaisyu_phase = 3;
}
break;
case 3:
//後進->停止
//リミット押されるまで低速後進
if(pulse <= 500) {
arm_motor[0] = 0x4C;
//kaisyu_phase = 4;
}
//リミット押されたら停止
if(kaisyu_mae_limit == 1) {
arm_motor[0] = 0x80;
kaisyu_phase = 4;
phase = next_phase;
}
break;
default:
arm_motor[0] = 0x80;
break;
}
//回収MDへ書き込み
i2c.write(0x18, arm_motor, 1);
wait_us(20);
}
void kaisyu_nobasu(int next_phase) {
//前進->減速
//3000pulseまで高速前進
if(arm_pulse < 3000) {
arm_motor[0] = 0xFF;
}
//3000pulse超えたら低速前進
else if(arm_pulse >= 3000) {
arm_motor[0] = 0xB3;
}
//3600pulse超えたら停止
else if(arm_pulse >= 3600) {
arm_motor[0] = 0x80;
phase = next_phase;
} else {
arm_motor[0] = 0x80;
}
//後ろのリミットが押されたら強制停止
if(kaisyu_usiro_limit == 1) {
arm_motor[0] = 0x80;
phase = next_phase;
}
//回収MDへ書き込み
i2c.write(0x18, arm_motor, 1);
wait_us(20);
}
void kaisyu_hiku(int next_phase) {
//後進->減速
//500pulseより大きい範囲で高速後進
if(arm_pulse > 500) {
arm_motor[0] = 0x00;
}
//500pulse以下になったら低速後進
else if(arm_pulse <= 500) {
arm_motor[0] = 0x4C;
}
//0pulse以下で停止
else if(arm_pulse <= 0) {
arm_motor[0] = 0x80;
phase = next_phase;
} else {
arm_motor[0] = 0x80;
}
//後ろのリミットが押されたら強制停止
if(kaisyu_mae_limit == 1) {
arm_motor[0] = 0x80;
phase = next_phase;
}
//回収MDへ書き込み
i2c.write(0x18, arm_motor, 1);
wait_us(20);
}
void tyokudo(int pulse, int next_phase) {
switch(tyokudo_phase) {
case 0:
//前進->減速
/* エンコーダー読まずにリミットだけ(修正必須) */
//3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
if(tyokudo_mae_limit == 0) {
//2000pulseまで高速前進
if(pulse < 2000) {
arm_motor[0] = 0xC0;
drop_motor[0] = 0xE6;
}
//2000pulse以上で低速前進
else if(pulse >= 2000) {
arm_motor[0] = 0xC0;
drop_motor[0] = 0xE6;
}
//パルスが3600を終えたらアームのみ強制停止
else if(pulse > 3600) {
arm_motor[0] = 0x80;
drop_motor[0] = 0xE6;
}
//後ろのリミットが押されたら強制停止
if(kaisyu_usiro_limit == 1) {
arm_motor[0] = 0x80;
}
}
//直動の前リミットが押されたら
else if(tyokudo_mae_limit == 1) {
//高速後進
arm_motor[0] = 0x4C;
drop_motor[0] = 0x00;
tyokudo_phase = 1;
}
break;
case 1:
//後進->停止
if(tyokudo_usiro_limit == 1) {
drop_motor[0] = 0x80;
if(kaisyu_mae_limit == 1) {
arm_motor[0] = 0x80;
tyokudo_phase = 2;
phase = next_phase;
}
}
if(kaisyu_mae_limit == 1) {
arm_motor[0] = 0x80;
if(tyokudo_usiro_limit == 1) {
drop_motor[0] = 0x80;
tyokudo_phase = 2;
phase = next_phase;
}
}
break;
default:
arm_motor[0] = 0x80;
drop_motor[0] = 0x80;
break;
}
//回収MD・排出MDへ書き込み
i2c.write(0x18, arm_motor, 1);
i2c.write(0x20, drop_motor, 1);
wait_us(20);
}
void tyokudo_nobasu(int next_phase) {
if(tyokudo_mae_limit == 0) {
drop_motor[0] = 0xFF;
//2000pulseまで高速前進
if(arm_pulse < 2000) {
arm_motor[0] = 0xFF;
}
//2000pulse以上で低速前進
else if(arm_pulse >= 2000) {
arm_motor[0] = 0xB3;
}
//パルスが2500を終えたらアームのみ強制停止
else if(arm_pulse > 2500) {
arm_motor[0] = 0x80;
} else {
arm_motor[0] = 0x80;
}
//後ろのリミットが押されたら強制停止
if(kaisyu_usiro_limit == 1) {
arm_motor[0] = 0x80;
}
}
else if(tyokudo_mae_limit == 1) {
drop_motor[0] = 0x80;
//2000pulseまで高速前進
if(arm_pulse < 2000) {
arm_motor[0] = 0xFF;
}
//2000pulse以上で低速前進
else if(arm_pulse >= 2000) {
arm_motor[0] = 0xB3;
}
//パルスが2500を終えたらアームのみ強制停止
else if(arm_pulse > 2500) {
arm_motor[0] = 0x80;
phase = next_phase;
} else {
arm_motor[0] = 0x80;
}
//後ろのリミットが押されたら強制停止
if(kaisyu_usiro_limit == 1) {
arm_motor[0] = 0x80;
phase = next_phase;
}
}
//回収MD・排出MDへ書き込み
i2c.write(0x18, arm_motor, 1);
i2c.write(0x20, drop_motor, 1);
wait_us(20);
}
void tyokudo_hiku(int next_phase) {
if(tyokudo_usiro_limit == 0) {
drop_motor[0] = 0x00;
//500pulseより大きい範囲で高速後進
if(arm_pulse > 500) {
arm_motor[0] = 0x00;
}
//500pulse以下になったら低速後進
else if(arm_pulse <= 500) {
arm_motor[0] = 0x4C;
}
//0pulse以下で停止
else if(arm_pulse <= 0) {
arm_motor[0] = 0x80;
} else {
arm_motor[0] = 0x80;
}
//後ろのリミットが押されたら強制停止
if(kaisyu_mae_limit == 1) {
arm_motor[0] = 0x80;
}
}
else if(tyokudo_usiro_limit == 1) {
drop_motor[0] = 0x80;
//500pulseより大きい範囲で高速後進
if(arm_pulse > 500) {
arm_motor[0] = 0x00;
}
//500pulse以下になったら低速後進
else if(arm_pulse <= 500) {
arm_motor[0] = 0x4C;
}
//0pulse以下で停止
else if(arm_pulse <= 0) {
arm_motor[0] = 0x80;
phase = next_phase;
} else {
arm_motor[0] = 0x80;
}
//後ろのリミットが押されたら強制停止
if(kaisyu_mae_limit == 1) {
arm_motor[0] = 0x80;
phase = next_phase;
}
}
//回収MD・排出MDへ書き込み
i2c.write(0x18, arm_motor, 1);
i2c.write(0x20, drop_motor, 1);
wait_us(20);
}
void arm_up(int next_phase) {
//両腕、上限リミットが押されてなかったら上昇
if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
right_arm_data[0] = 0x00; left_arm_data[0] = 0x00;
}
//右腕のみリミットが押されたら左腕のみ上昇
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
right_arm_data[0] = 0x80; left_arm_data[0] = 0x00;
}
//左腕のみリミットが押されたら右腕のみ上昇
else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
right_arm_data[0] = 0x00; left_arm_data[0] = 0x80;
}
//両腕、上限リミットが押されたら停止
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
phase = next_phase;
}
i2c.write(0x22, right_arm_data, 1);
i2c.write(0x24, left_arm_data, 1);
wait_us(20);
}
void fan_on(float first_wind_time, float second_wind_time, int next_phase) {
if(counter.read() <= first_wind_time) {
fan_data[0] = 0xFF;
servo_data[0] = 0x04;
}
else if((counter.read() > first_wind_time) && (counter.read() <= first_wind_time + second_wind_time)) {
fan_data[0] = 0xFF;
servo_data[0] = 0x03;
}
else if(counter.read() > first_wind_time + second_wind_time) {
fan_data[0] = 0x80;
servo_data[0] = 0x04;
phase = next_phase;
}
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
i2c.write(0x30, servo_data, 1);
wait_us(20);
}
void front(int target) {
front_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void back(int target) {
back_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void right(int target) {
right_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void left(int target) {
left_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void turn_right(int target) {
turn_right_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void turn_left(int target) {
turn_left_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void stop(void) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
}
void all_stop(void) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
arm_motor[0] = 0x80;
drop_motor[0] = 0x80;
right_arm_data[0] = 0x80;
left_arm_data[0] = 0x80;
fan_data[0] = 0x80;
servo_data[0] = 0x04;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data, 1, false);
i2c.write(0x18, arm_motor, 1);
i2c.write(0x20, drop_motor, 1);
i2c.write(0x22, right_arm_data, 1);
i2c.write(0x24, left_arm_data, 1);
i2c.write(0x26, fan_data, 1);
i2c.write(0x28, fan_data, 1);
i2c.write(0x30, servo_data, 1);
wait_us(20);
}
void front_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
front_migimae.setInputLimits(-2147483648, 2147483647);
front_migiusiro.setInputLimits(-2147483648, 2147483647);
front_hidarimae.setInputLimits(-2147483648, 2147483647);
front_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//正転(目標に達してない)
if((y_pulse1 < target) && (y_pulse2 < target)) {
front_migimae.setOutputLimits(0x84, 0xF5);
front_migiusiro.setOutputLimits(0x84, 0xF5);
front_hidarimae.setOutputLimits(0x84, 0xFF);
front_hidariusiro.setOutputLimits(0x84, 0xFF);
}
//停止(目標より行き過ぎ)
else if((y_pulse1 > target) && (y_pulse2 > target)) {
front_migimae.setOutputLimits(0x7C, 0x83);
front_migiusiro.setOutputLimits(0x7C, 0x83);
front_hidarimae.setOutputLimits(0x7C, 0x83);
front_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
front_migimae.setMode(AUTO_MODE);
front_migiusiro.setMode(AUTO_MODE);
front_hidarimae.setMode(AUTO_MODE);
front_hidariusiro.setMode(AUTO_MODE);
//目標値
front_migimae.setSetPoint(target);
front_migiusiro.setSetPoint(target);
front_hidarimae.setSetPoint(target);
front_hidariusiro.setSetPoint(target);
//センサ出力
front_migimae.setProcessValue(y_pulse1);
front_migiusiro.setProcessValue(y_pulse1);
front_hidarimae.setProcessValue(y_pulse2);
front_hidariusiro.setProcessValue(y_pulse2);
//制御量(計算結果)
migimae_data[0] = front_migimae.compute();
migiusiro_data[0] = front_migiusiro.compute();
hidarimae_data[0] = front_hidarimae.compute();
hidariusiro_data[0] = front_hidariusiro.compute();
//制御量をPWM値に変換
//正転(目標に達してない)
if((y_pulse1 < target) && (y_pulse2 < target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
//停止(目標より行き過ぎ)
else if((y_pulse1 > target) && (y_pulse2 > target)) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void back_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
back_migimae.setInputLimits(-2147483648, 2147483647);
back_migiusiro.setInputLimits(-2147483648, 2147483647);
back_hidarimae.setInputLimits(-2147483648, 2147483647);
back_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//逆転(目標に達してない)
if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
back_migimae.setOutputLimits(0x00, 0x7B);
back_migiusiro.setOutputLimits(0x00, 0x7B);
back_hidarimae.setOutputLimits(0x00, 0x70);
back_hidariusiro.setOutputLimits(0x00, 0x70);
//back_hidarimae.setOutputLimits(0x00, 0x7B);
//back_hidariusiro.setOutputLimits(0x00, 0x7B);
}
//停止(目標より行き過ぎ)
else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
back_migimae.setOutputLimits(0x7C, 0x83);
back_migiusiro.setOutputLimits(0x7C, 0x83);
back_hidarimae.setOutputLimits(0x7C, 0x83);
back_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
back_migimae.setMode(AUTO_MODE);
back_migiusiro.setMode(AUTO_MODE);
back_hidarimae.setMode(AUTO_MODE);
back_hidariusiro.setMode(AUTO_MODE);
//目標値
back_migimae.setSetPoint(target*-1);
back_migiusiro.setSetPoint(target*-1);
back_hidarimae.setSetPoint(target*-1);
back_hidariusiro.setSetPoint(target*-1);
//センサ出力
back_migimae.setProcessValue(y_pulse1*-1);
back_migiusiro.setProcessValue(y_pulse1*-1);
back_hidarimae.setProcessValue(y_pulse2*-1);
back_hidariusiro.setProcessValue(y_pulse2*-1);
//制御量(計算結果)
migimae_data[0] = back_migimae.compute();
migiusiro_data[0] = back_migiusiro.compute();
hidarimae_data[0] = back_hidarimae.compute();
hidariusiro_data[0] = back_hidariusiro.compute();
//制御量をPWM値に変換
//逆転(目標に達してない)
if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//停止(目標より行き過ぎ)
else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void right_PID(int target) {
//センサ出力値の最小、最大
right_migimae.setInputLimits(-2147483648, 2147483647);
right_migiusiro.setInputLimits(-2147483648, 2147483647);
right_hidarimae.setInputLimits(-2147483648, 2147483647);
right_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//右進(目標まで達していない)
if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
right_migimae.setOutputLimits(0x6A, 0x6C);
//right_migimae.setOutputLimits(0x7A, 0x7B);
right_migiusiro.setOutputLimits(0xFE, 0xFF);
right_hidarimae.setOutputLimits(0xEF, 0xF0);
//right_hidarimae.setOutputLimits(0xFE, 0xFF);
right_hidariusiro.setOutputLimits(0x7A, 0x7B);
}
//停止(目標より行き過ぎ)
else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
right_migimae.setOutputLimits(0x7C, 0x83);
right_migiusiro.setOutputLimits(0x7C, 0x83);
right_hidarimae.setOutputLimits(0x7C, 0x83);
right_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
right_migimae.setMode(AUTO_MODE);
right_migiusiro.setMode(AUTO_MODE);
right_hidarimae.setMode(AUTO_MODE);
right_hidariusiro.setMode(AUTO_MODE);
//目標値
right_migimae.setSetPoint(target*-1);
right_migiusiro.setSetPoint(target*-1);
right_hidarimae.setSetPoint(target*-1);
right_hidariusiro.setSetPoint(target*-1);
//センサ出力
right_migimae.setProcessValue(x_pulse1*-1);
right_migiusiro.setProcessValue(x_pulse2*-1);
right_hidarimae.setProcessValue(x_pulse1*-1);
right_hidariusiro.setProcessValue(x_pulse2*-1);
//制御量(計算結果)
migimae_data[0] = right_migimae.compute();
migiusiro_data[0] = right_migiusiro.compute();
hidarimae_data[0] = right_hidarimae.compute();
hidariusiro_data[0] = right_hidariusiro.compute();
//制御量をPWM値に変換
//右進(目標まで達していない)
if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//左進(目標より行き過ぎ)
else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void left_PID(int target) {
//センサ出力値の最小、最大
left_migimae.setInputLimits(-2147483648, 2147483647);
left_migiusiro.setInputLimits(-2147483648, 2147483647);
left_hidarimae.setInputLimits(-2147483648, 2147483647);
left_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//左進(目標まで達していない)
if((x_pulse1 < target) && (x_pulse2 < target)) {
left_migimae.setOutputLimits(0xEC, 0xED);
left_migiusiro.setOutputLimits(0x7A, 0x7B);
left_hidarimae.setOutputLimits(0x6E, 0x6F);
left_hidariusiro.setOutputLimits(0xFE, 0xFF);
}
//停止(目標より行き過ぎ)
else if((x_pulse1 > target) && (x_pulse2 > target)) {
left_migimae.setOutputLimits(0x7C, 0x83);
left_migiusiro.setOutputLimits(0x7C, 0x83);
left_hidarimae.setOutputLimits(0x7C, 0x83);
left_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
left_migimae.setMode(AUTO_MODE);
left_migiusiro.setMode(AUTO_MODE);
left_hidarimae.setMode(AUTO_MODE);
left_hidariusiro.setMode(AUTO_MODE);
//目標値
left_migimae.setSetPoint(target);
left_migiusiro.setSetPoint(target);
left_hidarimae.setSetPoint(target);
left_hidariusiro.setSetPoint(target);
//センサ出力
left_migimae.setProcessValue(x_pulse1);
left_migiusiro.setProcessValue(x_pulse2);
left_hidarimae.setProcessValue(x_pulse1);
left_hidariusiro.setProcessValue(x_pulse2);
//制御量(計算結果)
migimae_data[0] = left_migimae.compute();
migiusiro_data[0] = left_migiusiro.compute();
hidarimae_data[0] = left_hidarimae.compute();
hidariusiro_data[0] = left_hidariusiro.compute();
//制御量をPWM値に変換
//左進(目標まで達していない)
if((x_pulse1 < target) && (x_pulse2 < target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
//停止(目標より行き過ぎ)
else if((x_pulse1 > target) && (x_pulse2 > target)) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void turn_right_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
turn_right_migimae.setInputLimits(-2147483648, 2147483647);
turn_right_migiusiro.setInputLimits(-2147483648, 2147483647);
turn_right_hidarimae.setInputLimits(-2147483648, 2147483647);
turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//右旋回(目標に達してない)
if(sum_pulse < target) {
turn_right_migimae.setOutputLimits(0x10, 0x7B);
turn_right_migiusiro.setOutputLimits(0x10, 0x7B);
turn_right_hidarimae.setOutputLimits(0x94, 0xFF);
turn_right_hidariusiro.setOutputLimits(0x94, 0xFF);
}
//停止(目標より行き過ぎ)
else if(sum_pulse > target) {
turn_right_migimae.setOutputLimits(0x7C, 0x83);
turn_right_migiusiro.setOutputLimits(0x7C, 0x83);
turn_right_hidarimae.setOutputLimits(0x7C, 0x83);
turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
turn_right_migimae.setMode(AUTO_MODE);
turn_right_migiusiro.setMode(AUTO_MODE);
turn_right_hidarimae.setMode(AUTO_MODE);
turn_right_hidariusiro.setMode(AUTO_MODE);
//目標値
turn_right_migimae.setSetPoint(target);
turn_right_migiusiro.setSetPoint(target);
turn_right_hidarimae.setSetPoint(target);
turn_right_hidariusiro.setSetPoint(target);
//センサ出力
turn_right_migimae.setProcessValue(sum_pulse);
turn_right_migiusiro.setProcessValue(sum_pulse);
turn_right_hidarimae.setProcessValue(sum_pulse);
turn_right_hidariusiro.setProcessValue(sum_pulse);
//制御量(計算結果)
migimae_data[0] = turn_right_migimae.compute();
migiusiro_data[0] = turn_right_migiusiro.compute();
hidarimae_data[0] = turn_right_hidarimae.compute();
hidariusiro_data[0] = turn_right_hidariusiro.compute();
//制御量をPWM値に変換
//右旋回(目標に達してない)
if(sum_pulse < target) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
//停止(目標より行き過ぎ)
else if(sum_pulse > target) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void turn_left_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
turn_left_migimae.setInputLimits(-2147483648, 2147483647);
turn_left_migiusiro.setInputLimits(-2147483648, 2147483647);
turn_left_hidarimae.setInputLimits(-2147483648, 2147483647);
turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//左旋回(目標に達してない)
if(sum_pulse < target) {
turn_left_migimae.setOutputLimits(0x94, 0xFF);
turn_left_migiusiro.setOutputLimits(0x94, 0xFF);
turn_left_hidarimae.setOutputLimits(0x10, 0x7B);
turn_left_hidariusiro.setOutputLimits(0x10, 0x7B);
}
//停止(目標より行き過ぎ)
else if(sum_pulse > target) {
turn_left_migimae.setOutputLimits(0x7C, 0x83);
turn_left_migiusiro.setOutputLimits(0x7C, 0x83);
turn_left_hidarimae.setOutputLimits(0x7C, 0x83);
turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
turn_left_migimae.setMode(AUTO_MODE);
turn_left_migiusiro.setMode(AUTO_MODE);
turn_left_hidarimae.setMode(AUTO_MODE);
turn_left_hidariusiro.setMode(AUTO_MODE);
//目標値
turn_left_migimae.setSetPoint(target);
turn_left_migiusiro.setSetPoint(target);
turn_left_hidarimae.setSetPoint(target);
turn_left_hidariusiro.setSetPoint(target);
//センサ出力
turn_left_migimae.setProcessValue(sum_pulse);
turn_left_migiusiro.setProcessValue(sum_pulse);
turn_left_hidarimae.setProcessValue(sum_pulse);
turn_left_hidariusiro.setProcessValue(sum_pulse);
//制御量(計算結果)
migimae_data[0] = turn_left_migimae.compute();
migiusiro_data[0] = turn_left_migiusiro.compute();
hidarimae_data[0] = turn_left_hidarimae.compute();
hidariusiro_data[0] = turn_left_hidariusiro.compute();
//制御量をPWM値に変換
//左旋回(目標に達してない)
if(sum_pulse < target) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//左旋回(目標より行き過ぎ)
else if(sum_pulse > target) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}