2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
main.cpp
- Committer:
- yuron
- Date:
- 2019-09-18
- Revision:
- 20:ac4954be1fe0
- Parent:
- 19:f17d2e585973
- Child:
- 21:123520676121
File content as of revision 20:ac4954be1fe0:
/* ------------------------------------------------------------------- */
/* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */
/* Nucleo Type: F446RE */
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
/* Sensor: encorder*4 */
/* ------------------------------------------------------------------- */
/* ファンとサーボの動作を追加した */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
//直進補正の為の前後・左右の回転差の許容値
#define wheel_difference 100
#define RED 0
#define BLUE 1
//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(30000000.0, 0.0, 0.0, 0.001);
PID turn_right_migiusiro(30000000.0, 0.0, 0.0, 0.001);
PID turn_right_hidarimae(30000000.0, 0.0, 0.0, 0.001);
PID turn_right_hidariusiro(30000000.0, 0.0, 0.0, 0.001);
//左旋回
PID turn_left_migimae(30000000.0, 0.0, 0.0, 0.001);
PID turn_left_migiusiro(30000000.0, 0.0, 0.0, 0.001);
PID turn_left_hidarimae(30000000.0, 0.0, 0.0, 0.001);
PID turn_left_hidariusiro(30000000.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);
//遠隔非常停止ユニットLED
AnalogOut myled(A2);
DigitalIn start_switch(PB_12);
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_5, PB_4 , NC, 624);
//移動後n秒停止タイマー
Timer counter;
//エンコーダ値格納変数
int x_pulse1, x_pulse2, y_pulse1, y_pulse2;
//操作の段階変数
unsigned int phase = 0;
int kaisyu_phase = 0;
int tyokudo_phase = 0;
unsigned int start_zone = 1;
bool zone = RED;
//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_moter[1], drop_moter[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_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 = 0xFF;
int masked_lower_back_limit_data = 0xFF;
int masked_lower_right_limit_data = 0xFF;
int masked_kaisyu_limit_data = 0xFF;
int masked_right_arm_lower_limit_data = 0xFF;
int masked_right_arm_upper_limit_data = 0xFF;
int masked_left_arm_lower_limit_data = 0xFF;
int masked_left_arm_upper_limit_data = 0xFF;
int masked_tyokudo_mae_limit_data = 0xFF;
int masked_tyokudo_usiro_limit_data = 0xFF;
//関数のプロトタイプ宣言
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);
void tyokudo(int pulse);
void arm_up(void);
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 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);
void dondonkasoku(void);
int main(void) {
init();
init_send();
//とりあえず(後で消してね)
zone = BLUE;
while(1) {
get_pulses();
print_pulses();
get_emergency();
read_limit();
//if(start_switch == 1) {
//tyokudo(arm_enc.getPulses());
//}
//青ゾーン
if(zone == BLUE) {
switch(phase) {
//スタート位置へセット
case 0:
//リミットが洗濯物台に触れているか
if(right_limit == 3) {
USR_LED1 = 1;
//スタートスイッチが押されたか
if(start_switch == 1) {
wheel_reset();
phase = 1;
}
} else {
USR_LED1 = 0;
}
break;
//回収
case 1:
//回収アームのリミットが押されているか
if(kaisyu_limit == 1) {
kaisyu(arm_enc.getPulses());
USR_LED2 = 1;
} else {
USR_LED2 = 0;
}
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
//1秒停止
case 2:
stop();
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
counter.start();
if(counter.read() > 1.0f) {
phase = 3;
wheel_reset();
}
break;
//左移動
case 3:
counter.reset();
left(12000);
if((x_pulse1 > 12000) && (x_pulse2 > 12000)) {
phase = 4;
}
break;
//1秒停止
case 4:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 5;
wheel_reset();
}
break;
//右旋回(180°)
case 5:
counter.reset();
turn_right(518);
if(x_pulse2 > 518) {
phase = 6;
}
break;
//1秒停止
case 6:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 7;
wheel_reset();
}
break;
//ちょっくら右移動
case 7:
counter.reset();
right(-1000);
if((x_pulse1*-1 > 500) && (x_pulse2*-1 > 500)) {
true_migimae_data[0] = 0x94;
true_migiusiro_data[0] = 0x10;
true_hidarimae_data[0] = 0x10;
true_hidariusiro_data[0] = 0x94;
}
if(right_limit == 3) {
phase = 8;
}
break;
//1秒停止
case 8:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 9;
wheel_reset();
}
break;
//排出
case 9:
counter.reset();
tyokudo(arm_enc.getPulses());
//ここに排出動作が来るが今回は飛ばす
//phase = 10;
break;
//排出機構引っ込める
case 10:
//ここに排出機構引っ込める動作が来るが今回は飛ばす
phase = 11;
break;
//1秒停止
case 11:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 12;
wheel_reset();
}
break;
//前進
case 12:
counter.reset();
front(3000);
if((y_pulse1 > 3000) && (y_pulse2 > 3000)) {
phase = 13;
}
break;
//1秒停止
case 13:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 14;
wheel_reset();
}
break;
//右移動
case 14:
counter.reset();
right(-4000);
if((x_pulse1*-1 > 3000) && (x_pulse2*-1 > 3000)) {
true_migimae_data[0] = 0x94;
true_migiusiro_data[0] = 0x10;
true_hidarimae_data[0] = 0x10;
true_hidariusiro_data[0] = 0x94;
}
if(right_limit == 3) {
phase = 15;
}
break;
//シーツ装填
case 15:
if(start_switch == 1) {
phase = 16;
} else {
stop();
}
break;
//竿のラインまで前進
case 16:
counter.reset();
front(21500);
if((y_pulse1 > 21500) && (y_pulse2 > 21500)) {
phase = 17;
}
break;
//1秒停止
case 17:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 18;
wheel_reset();
}
break;
//掛けるところまで左移動
case 18:
counter.reset();
left(10000);
if((x_pulse1 > 10000) && (x_pulse2 > 10000)) {
phase = 19;
}
break;
//1秒停止
case 19:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 20;
wheel_reset();
}
break;
//妨害防止の右旋回
case 20:
counter.reset();
turn_right(300);
if(x_pulse2 > 300) {
phase = 21;
}
break;
//1秒停止
case 21:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 22;
wheel_reset();
}
break;
//アームアップ
case 22:
stop();
counter.reset();
arm_up();
break;
//シーツを掛ける
case 23:
counter.start();
//5秒間ファン送風
if(counter.read() <= 4.0f) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
}
//4~5秒の間でサーボを放す
else if((counter.read() > 4.0f) && (counter.read() <= 5.0f)) {
fan_data[0] = 0xFF;
i2c.write(0x26, fan_data, 1);
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
}
//5秒過ぎたらファン停止
else if(counter.read() > 5.0f) {
fan_data[0] = 0x80;
i2c.write(0x26, fan_data, 1);
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
phase = 24;
}
break;
//終了っ！(守衛さん風)
case 24:
//駆動系統OFF
emergency = 0;
break;
default:
//駆動系統OFF
emergency = 0;
break;
}
}
}
}
void init(void) {
//通信ボーレートの設定
pc.baud(460800);
limit_serial.baud(115200);
start_switch.mode(PullUp);
//非常停止関連
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;
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;
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();
}
void print_pulses(void) {
pc.printf("%d\n\r", tyokudo_phase);
//pc.printf("%d\n\r", arm_enc.getPulses());
//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\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]);
}
void get_emergency(void) {
if(RDATA == '1') {
myled = 1;
emergency = 1;
}
else if(RDATA == '9'){
myled = 0.2;
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_limit_data = lower_limit_data & 0b01000000;
//上リミット基板からのデータのマスク処理
masked_right_arm_lower_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_limit_data) {
//押された
case 0b00000000:
kaisyu_limit = 1;
break;
case 0b01000000:
kaisyu_limit = 0;
break;
default:
kaisyu_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_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) {
switch (kaisyu_phase) {
case 0:
//前進->減速
//3000pulseまで高速前進
if(pulse < 3000) {
arm_moter[0] = 0xFF;
//kaisyu_phase = 1;
}
//3000pulse超えたら低速前進
else if(pulse >= 3000) {
arm_moter[0] = 0xB3;
kaisyu_phase = 1;
}
break;
case 1:
//前進->停止->後進
//3600pulseまで低速前進
if(pulse < 3600) {
arm_moter[0] = 0xB3;
//kaisyu_phase = 2;
}
//3600pulse超えたら停止
else if(pulse >= 3600) {
arm_moter[0] = 0x80;
//1秒待ってから引っ込める
counter.start();
if(counter.read() > 1.0f) {
kaisyu_phase = 2;
}
}
break;
case 2:
//後進->減速
//500pulseまで高速後進
counter.reset();
if(pulse > 500) {
arm_moter[0] = 0x00;
//kaisyu_phase = 3;
}
//500pulse以下になったら低速後進
else if(pulse <= 500) {
arm_moter[0] = 0x4C;
kaisyu_phase = 3;
}
break;
case 3:
//後進->停止
//リミット押されるまで低速後進
if(pulse <= 500) {
arm_moter[0] = 0x4C;
//kaisyu_phase = 4;
}
//リミット押されたら停止
if(kaisyu_limit == 1) {
arm_moter[0] = 0x80;
kaisyu_phase = 4;
phase = 2;
}
break;
default:
arm_moter[0] = 0x80;
break;
}
//回収MDへ書き込み
i2c.write(0x18, arm_moter, 1);
}
void tyokudo(int pulse) {
switch(tyokudo_phase) {
case 0:
//前進->減速
//3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
if(tyokudo_mae_limit == 0) {
//2000pulseまで高速前進
if(pulse < 2000) {
arm_moter[0] = 0xCD;
drop_moter[0] = 0xE6;
}
//2000pulse以上で低速前進
else if(pulse >= 2000) {
arm_moter[0] = 0xC0;
drop_moter[0] = 0xCD;
}
//パルスが3600を終えたらアームのみ強制停止
else if(pulse > 3600) {
arm_moter[0] = 0x80;
drop_moter[0] = 0xCD;
}
}
//直動の前リミットが押されたら
else if(tyokudo_mae_limit == 1) {
//直動のみ強制停止
drop_moter[0] = 0x80;
//2000pulseまで高速前進
if(pulse < 2000) {
arm_moter[0] = 0xCD;
}
//2000pulse以上で低速前進
else if(pulse >= 2000) {
arm_moter[0] = 0xC0;
}
//パルスが3600を終えたらアームも強制停止
else if(pulse > 3600) {
arm_moter[0] = 0x80;
tyokudo_phase = 1;
}
}
break;
case 1:
//後進->減速
//1600pulseより大きいとき高速後進
if(pulse > 1600) {
if(kaisyu_limit == 0) {
arm_moter[0] = 0x00;
}
//リミットが押されたら強制停止
else if(kaisyu_limit == 1) {
arm_moter[0] = 0x80;
}
if(tyokudo_usiro_limit == 0) {
drop_moter[0] = 0x19;
}
//リミットが押されたら強制停止
else if(tyokudo_usiro_limit == 1) {
drop_moter[0] = 0x80;
}
}
//500pulse以下でphase2へ移行
else if(pulse <= 500) {
tyokudo_phase = 2;
}
break;
case 2:
//後進->停止
//直動後リミットが押されるまで後進
if(tyokudo_usiro_limit == 0) {
drop_moter[0] = 0x19;
}
//直動後リミットが押されたら停止
else if(tyokudo_usiro_limit == 1) {
drop_moter[0] = 0x80;
}
//リミット押されるまで低速後進
if(kaisyu_limit == 0) {
arm_moter[0] = 0x4C;
}
else if(kaisyu_limit == 1) {
arm_moter[1] = 0x80;
tyokudo_phase = 3;
phase = 10;
}
break;
default:
arm_moter[0] = 0x80;
drop_moter[0] = 0x80;
break;
}
i2c.write(0x18, arm_moter, 1);
i2c.write(0x20, drop_moter, 1);
}
void arm_up(void) {
//両腕、上限リミットが押されてなかったら上昇
if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
right_arm_data[0] = 0xFF; left_arm_data[0] = 0xFF;
}
//右腕のみリミットが押されたら左腕のみ上昇
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
right_arm_data[0] = 0x80; left_arm_data[0] = 0xFF;
}
//左腕のみリミットが押されたら右腕のみ上昇
else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
right_arm_data[0] = 0xFF; 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 = 23;
}
i2c.write(0x22, right_arm_data, 1);
i2c.write(0x24, left_arm_data, 1);
}
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 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, 0xF7);
front_migiusiro.setOutputLimits(0x84, 0xF7);
//front_migimae.setOutputLimits(0x84, 0xFF);
//front_migiusiro.setOutputLimits(0x84, 0xFF);
front_hidarimae.setOutputLimits(0x84, 0xFF);
front_hidariusiro.setOutputLimits(0x84, 0xFF);
}
//左側が前に出ちゃった♥(右側だけ回して左側は停止)
else if((y_pulse1 + wheel_difference) < y_pulse2) {
front_migimae.setOutputLimits(0x84, 0xFF);
front_migiusiro.setOutputLimits(0x84, 0xFF);
front_hidarimae.setOutputLimits(0x7C, 0x83);
front_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//右側が前に出ちゃった♥(左側だけ回して右側は停止)
else if(y_pulse1 > (y_pulse2 + wheel_difference)) {
front_migimae.setOutputLimits(0x7C, 0x83);
front_migiusiro.setOutputLimits(0x7C, 0x83);
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 + wheel_difference) < y_pulse2) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
//右側が前に出ちゃった♥(左側だけ回して右側は停止)
else if(y_pulse1 > (y_pulse2 + wheel_difference)) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
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, 0x73);
//back_hidariusiro.setOutputLimits(0x00, 0x73);
back_hidarimae.setOutputLimits(0x00, 0x7B);
back_hidariusiro.setOutputLimits(0x00, 0x7B);
}
//左側が後に出ちゃった♥(右側だけ回して左側は停止)
else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){
back_migimae.setOutputLimits(0x00, 0x7B);
back_migiusiro.setOutputLimits(0x00, 0x7B);
back_hidarimae.setOutputLimits(0x7C, 0x83);
back_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//右側が後に出ちゃった♥(左側だけ回して右側は停止)
else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){
back_migimae.setOutputLimits(0x7C, 0x83);
back_migiusiro.setOutputLimits(0x7C, 0x83);
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 + wheel_difference) < y_pulse2*-1){
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
//右側が後に出ちゃった♥(左側だけ回して右側は停止)
else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
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(0x00, 0x6C);
right_migimae.setOutputLimits(0x00, 0x7B);
right_migiusiro.setOutputLimits(0x84, 0xFF);
//right_hidarimae.setOutputLimits(0x84, 0xF0);
right_hidarimae.setOutputLimits(0x84, 0xFF);
right_hidariusiro.setOutputLimits(0x00, 0x7B);
}
//前側が右に出ちゃった♥(後側だけ回して前側は停止)
else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){
right_migimae.setOutputLimits(0x7C, 0x83);
right_migiusiro.setOutputLimits(0x00, 0x7B);
right_hidarimae.setOutputLimits(0x7C, 0x83);
right_hidariusiro.setOutputLimits(0x84, 0xFF);
}
//後側が右に出ちゃった♥(前側だけ回して後側は停止)
else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){
right_migimae.setOutputLimits(0x84, 0xFF);
right_migiusiro.setOutputLimits(0x7C, 0x83);
right_hidarimae.setOutputLimits(0x00, 0x7B);
right_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//停止(目標より行き過ぎ)
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 > (x_pulse2*-1 + wheel_difference)){
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//後側が右に出ちゃった♥(前側だけ回して後側は停止)
else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = 0x80;
}
//左進(目標より行き過ぎ)
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(0x84, 0xED);
left_migiusiro.setOutputLimits(0x00, 0x7B);
left_hidarimae.setOutputLimits(0x00, 0x7B);
left_hidariusiro.setOutputLimits(0x84, 0xFF);
}
//前側が左に出ちゃった♥(後側だけ回して前側は停止)
else if(x_pulse1 > (x_pulse2 + wheel_difference)){
left_migimae.setOutputLimits(0x7C, 0x83);
left_migiusiro.setOutputLimits(0x7C, 0x83);
left_hidarimae.setOutputLimits(0x10, 0x7B);
left_hidariusiro.setOutputLimits(0x94, 0xFF);
}
//後側が左に出ちゃった♥(前側だけ回して後側は停止)
else if((x_pulse1 + wheel_difference) < x_pulse2){
left_migimae.setOutputLimits(0x94, 0xFF);
left_migiusiro.setOutputLimits(0x10, 0x7B);
left_hidarimae.setOutputLimits(0x7C, 0x83);
left_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//停止(目標より行き過ぎ)
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 > (x_pulse2 + wheel_difference)){
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
//後側が左に出ちゃった♥(前側だけ回して後側は停止)
else if((x_pulse1 + wheel_difference) < x_pulse2){
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
//停止(目標より行き過ぎ)
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(x_pulse2 < 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(x_pulse2 > 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(x_pulse2);
turn_right_migiusiro.setProcessValue(x_pulse2);
turn_right_hidarimae.setProcessValue(x_pulse2);
turn_right_hidariusiro.setProcessValue(x_pulse2);
//制御量(計算結果)
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(x_pulse2 < 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(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_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(x_pulse1 < 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(x_pulse1 > 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(x_pulse1);
turn_left_migiusiro.setProcessValue(x_pulse1);
turn_left_hidarimae.setProcessValue(x_pulse1);
turn_left_hidariusiro.setProcessValue(x_pulse1);
//制御量(計算結果)
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(x_pulse1 < 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(x_pulse1 > target) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
}
void dondonkasoku(void) {
//どんどん加速(正転)
for(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++){
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);
wait(0.05);
}
//どんどん減速(正転)
for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--){
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);
wait(0.05);
}
//だんだん加速(逆転)
for(init_send_data[0] = 0x7B; init_send_data[0] > 0x00; init_send_data[0]--){
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);
wait(0.05);
}
//だんだん減速(逆転)
for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++){
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);
wait(0.05);
}
}