![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
main.cpp
- Committer:
- yuron
- Date:
- 2019-08-26
- Revision:
- 14:ab89b6cd9719
- Parent:
- 13:93321c73df60
- Child:
- 15:d022288aec51
File content as of revision 14:ab89b6cd9719:
/* ------------------------------------------------------------------- */ /* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */ /* Nucleo Type: F446RE */ /* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */ /* Sensor: encorder*4 */ /* ------------------------------------------------------------------- */ /* PID関数を各方向毎に追加した */ /* ------------------------------------------------------------------- */ #include "mbed.h" #include "math.h" #include "QEI.h" #include "PID.h" //PIDGain of wheels #define Kp 4500000.0 //#define Kp 10000000.0 #define Ti 0.0 #define Td 0.0 PID migimae(Kp, Ti, Td, 0.001); PID migiusiro(Kp, Ti, Td, 0.001); PID hidarimae(Kp, Ti, Td, 0.001); PID hidariusiro(Kp, Ti, Td, 0.001); //前進 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(4500000.0, 0.0, 0.0, 0.001); PID right_migiusiro(4500000.0, 0.0, 0.0, 0.001); PID right_hidarimae(4500000.0, 0.0, 0.0, 0.001); PID right_hidariusiro(4500000.0, 0.0, 0.0, 0.001); //左進 PID left_migimae(4500000.0, 0.0, 0.0, 0.001); PID left_migiusiro(4500000.0, 0.0, 0.0, 0.001); PID left_hidarimae(4500000.0, 0.0, 0.0, 0.001); PID left_hidariusiro(4500000.0, 0.0, 0.0, 0.001); //右旋回 PID turn_right_migimae(4500000.0, 0.0, 0.0, 0.001); PID turn_right_migiusiro(4500000.0, 0.0, 0.0, 0.001); PID turn_right_hidarimae(4500000.0, 0.0, 0.0, 0.001); PID turn_right_hidariusiro(4500000.0, 0.0, 0.0, 0.001); //左旋回 PID turn_left_migimae(4500000.0, 0.0, 0.0, 0.001); PID turn_left_migiusiro(4500000.0, 0.0, 0.0, 0.001); PID turn_left_hidarimae(4500000.0, 0.0, 0.0, 0.001); PID turn_left_hidariusiro(4500000.0, 0.0, 0.0, 0.001); //MDとの通信ポート I2C i2c(PB_9, PB_8); //SDA, SCL //PCとの通信ポート Serial pc(USBTX, USBRX); //TX, RX //12V停止信号ピン DigitalOut emergency(D11); DigitalOut USR_LED1(PC_9); DigitalOut USR_LED2(PC_8); DigitalOut USR_LED3(PC_6); DigitalOut USR_LED4(PC_5); 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 wheel1(D2, D3, NC, 624); //QEI wheel2(D5, D4, NC, 624); //エンコーダ値格納変数 int x_pulse1, x_pulse2, y_pulse1, y_pulse2; //操作の段階変数 unsigned int phase = 0; //MD送信データ変数 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]; //関数のプロトタイプ宣言 void init(void); void init_send(void); void get_pulses(void); void print_pulses(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 dondonkasoku(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(); while(1) { get_pulses(); print_pulses(); front(500); //back(500); //right(500); //left(500); //turn_right(500); //turn_left(500); } } void init(void) { //緊急停止用信号ピンをLow emergency = 0; //USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1; //通信ボーレートの設定 //pc.baud(460800); pc.baud(9600); 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; } 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); wait(0.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("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2); } 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 front_PID(int target) { //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) migimae.setInputLimits(-2147483648, 2147483647); migiusiro.setInputLimits(-2147483648, 2147483647); hidarimae.setInputLimits(-2147483648, 2147483647); hidariusiro.setInputLimits(-2147483648, 2147483647); //制御量の最小、最大 //正転(目標に達してない) if((y_pulse1 < target) && (y_pulse2 < target)) { migimae.setOutputLimits(0x84, 0xFF); migiusiro.setOutputLimits(0x84, 0xFF); hidarimae.setOutputLimits(0x84, 0xFF); hidariusiro.setOutputLimits(0x84, 0xFF); } //逆転(目標より行き過ぎ) else if((y_pulse1 > target) && (y_pulse2 > target)) { migimae.setOutputLimits(0x00, 0x7B); migiusiro.setOutputLimits(0x00, 0x7B); hidarimae.setOutputLimits(0x00, 0x7B); hidariusiro.setOutputLimits(0x00, 0x7B); } else { migimae.setOutputLimits(0x7C, 0x83); migiusiro.setOutputLimits(0x7C, 0x83); hidarimae.setOutputLimits(0x7C, 0x83); hidariusiro.setOutputLimits(0x7C, 0x83); } //よくわからんやつ migimae.setMode(AUTO_MODE); migiusiro.setMode(AUTO_MODE); hidarimae.setMode(AUTO_MODE); hidariusiro.setMode(AUTO_MODE); //目標値 migimae.setSetPoint(target); migiusiro.setSetPoint(target); hidarimae.setSetPoint(target); hidariusiro.setSetPoint(target); //センサ出力 migimae.setProcessValue(y_pulse1); migiusiro.setProcessValue(y_pulse1); hidarimae.setProcessValue(y_pulse2); hidariusiro.setProcessValue(y_pulse2); //制御量(計算結果) migimae_data[0] = migimae.compute(); migiusiro_data[0] = migiusiro.compute(); hidarimae_data[0] = hidarimae.compute(); hidariusiro_data[0] = 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] = 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 { 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型が持てる範囲に設定) migimae.setInputLimits(-2147483648, 2147483647); migiusiro.setInputLimits(-2147483648, 2147483647); hidarimae.setInputLimits(-2147483648, 2147483647); hidariusiro.setInputLimits(-2147483648, 2147483647); //制御量の最小、最大 //逆転(目標に達してない) if((abs(y_pulse1) < abs(target)) || (abs(y_pulse2) < abs(target))) { migimae.setOutputLimits(0x00, 0x7B); migiusiro.setOutputLimits(0x00, 0x7B); hidarimae.setOutputLimits(0x00, 0x7B); hidariusiro.setOutputLimits(0x0, 0x7B); } //正転(目標より行き過ぎ) else if((abs(y_pulse1) > abs(target)) || (abs(y_pulse2) > abs(target))) { migimae.setOutputLimits(0x84, 0xFF); migiusiro.setOutputLimits(0x84, 0xFF); hidarimae.setOutputLimits(0x84, 0xFF); hidariusiro.setOutputLimits(0x84, 0xFF); } else { migimae.setOutputLimits(0x7C, 0x83); migiusiro.setOutputLimits(0x7C, 0x83); hidarimae.setOutputLimits(0x7C, 0x83); hidariusiro.setOutputLimits(0x7C, 0x83); } //よくわからんやつ migimae.setMode(AUTO_MODE); migiusiro.setMode(AUTO_MODE); hidarimae.setMode(AUTO_MODE); hidariusiro.setMode(AUTO_MODE); //目標値 migimae.setSetPoint(abs(target)); migiusiro.setSetPoint(abs(target)); hidarimae.setSetPoint(abs(target)); hidariusiro.setSetPoint(abs(target)); //センサ出力 migimae.setProcessValue(abs(y_pulse1)); migiusiro.setProcessValue(abs(y_pulse1)); hidarimae.setProcessValue(abs(y_pulse2)); hidariusiro.setProcessValue(abs(y_pulse2)); //制御量(計算結果) migimae_data[0] = migimae.compute(); migiusiro_data[0] = migiusiro.compute(); hidarimae_data[0] = hidarimae.compute(); hidariusiro_data[0] = hidariusiro.compute(); //制御量をPWM値に変換 //逆転(目標に達してない) if((abs(y_pulse1) < abs(target)) || (abs(y_pulse2) < abs(target))) { 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((abs(y_pulse1) > abs(target)) || (abs(y_pulse2) > abs(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 { 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) { //センサ出力値の最小、最大 migimae.setInputLimits(-2147483648, 2147483647); migiusiro.setInputLimits(-2147483648, 2147483647); hidarimae.setInputLimits(-2147483648, 2147483647); hidariusiro.setInputLimits(-2147483648, 2147483647); //制御量の最小、最大 //右進(目標まで達していない) if((x_pulse1 < target) && (x_pulse2 < target)) { migimae.setOutputLimits(0x00, 0x7B); migiusiro.setOutputLimits(0x84, 0xFF); hidarimae.setOutputLimits(0x84, 0xFF); hidariusiro.setOutputLimits(0x00, 0x7B); } //左進(目標より行き過ぎ) else if((x_pulse1 > target) && (x_pulse2 > target)) { migimae.setOutputLimits(0x84, 0xFF); migiusiro.setOutputLimits(0x00, 0x7B); hidarimae.setOutputLimits(0x00, 0x7B); hidariusiro.setOutputLimits(0x84, 0xFF); } //よくわからんやつ migimae.setMode(AUTO_MODE); migiusiro.setMode(AUTO_MODE); hidarimae.setMode(AUTO_MODE); hidariusiro.setMode(AUTO_MODE); //目標値 migimae.setSetPoint(target); migiusiro.setSetPoint(target); hidarimae.setSetPoint(target); hidariusiro.setSetPoint(target); //センサ出力 migimae.setProcessValue(x_pulse1); migiusiro.setProcessValue(x_pulse1); hidarimae.setProcessValue(x_pulse2); hidariusiro.setProcessValue(x_pulse2); //制御量(計算結果) migimae_data[0] = migimae.compute(); migiusiro_data[0] = migiusiro.compute(); hidarimae_data[0] = hidarimae.compute(); hidariusiro_data[0] = hidariusiro.compute(); //制御量をPWM値に変換 //右進(目標まで達していない) if((x_pulse1 < target) && (x_pulse2 < target)) { 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 > 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 { 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) { //センサ出力値の最小、最大 migimae.setInputLimits(-2147483648, 2147483647); migiusiro.setInputLimits(-2147483648, 2147483647); hidarimae.setInputLimits(-2147483648, 2147483647); hidariusiro.setInputLimits(-2147483648, 2147483647); //制御量の最小、最大 //左進(目標まで達していない) if((abs(x_pulse1) < abs(target)) && (abs(x_pulse2) < abs(target))) { migimae.setOutputLimits(0x84, 0xFF); migiusiro.setOutputLimits(0x00, 0x7B); hidarimae.setOutputLimits(0x00, 0x7B); hidariusiro.setOutputLimits(0x84, 0xFF); } //右進(目標より行き過ぎ) else if((abs(x_pulse1) > abs(target)) && (abs(x_pulse2) > abs(target))) { migimae.setOutputLimits(0x00, 0x7B); migiusiro.setOutputLimits(0x84, 0xFF); hidarimae.setOutputLimits(0x84, 0xFF); hidariusiro.setOutputLimits(0x00, 0x7B); } //よくわからんやつ migimae.setMode(AUTO_MODE); migiusiro.setMode(AUTO_MODE); hidarimae.setMode(AUTO_MODE); hidariusiro.setMode(AUTO_MODE); //目標値 migimae.setSetPoint(abs(target)); migiusiro.setSetPoint(abs(target)); hidarimae.setSetPoint(abs(target)); hidariusiro.setSetPoint(abs(target)); //センサ出力 migimae.setProcessValue(abs(x_pulse1)); migiusiro.setProcessValue(abs(x_pulse1)); hidarimae.setProcessValue(abs(x_pulse2)); hidariusiro.setProcessValue(abs(x_pulse2)); //制御量(計算結果) migimae_data[0] = migimae.compute(); migiusiro_data[0] = migiusiro.compute(); hidarimae_data[0] = hidarimae.compute(); hidariusiro_data[0] = hidariusiro.compute(); //制御量をPWM値に変換 //左進(目標まで達していない) if((abs(x_pulse1) < abs(target)) && (abs(x_pulse2) < abs(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((abs(x_pulse1) > abs(target)) && (abs(x_pulse2) > abs(target))) { 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 { 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型が持てる範囲に設定) migimae.setInputLimits(-2147483648, 2147483647); migiusiro.setInputLimits(-2147483648, 2147483647); hidarimae.setInputLimits(-2147483648, 2147483647); hidariusiro.setInputLimits(-2147483648, 2147483647); //制御量の最小、最大 //右旋回(目標に達してない) if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse2 < target)) { migimae.setOutputLimits(0x00, 0x7B); migiusiro.setOutputLimits(0x00, 0x7B); hidarimae.setOutputLimits(0x84, 0xFF); hidariusiro.setOutputLimits(0x84, 0xFF); } //左旋回(目標より行き過ぎ) else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) { migimae.setOutputLimits(0x84, 0xFF); migiusiro.setOutputLimits(0x84, 0xFF); hidarimae.setOutputLimits(0x00, 0x7B); hidariusiro.setOutputLimits(0x00, 0x7B); } //よくわからんやつ migimae.setMode(AUTO_MODE); migiusiro.setMode(AUTO_MODE); hidarimae.setMode(AUTO_MODE); hidariusiro.setMode(AUTO_MODE); //目標値 migimae.setSetPoint(target); migiusiro.setSetPoint(target); hidarimae.setSetPoint(target); hidariusiro.setSetPoint(target); //センサ出力 migimae.setProcessValue(x_pulse1); migiusiro.setProcessValue(x_pulse2); hidarimae.setProcessValue(y_pulse1); hidariusiro.setProcessValue(y_pulse2); //制御量(計算結果) migimae_data[0] = migimae.compute(); migiusiro_data[0] = migiusiro.compute(); hidarimae_data[0] = hidarimae.compute(); hidariusiro_data[0] = hidariusiro.compute(); //制御量をPWM値に変換 //右旋回(目標に達してない) if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse1 < 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_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > 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 { 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型が持てる範囲に設定) migimae.setInputLimits(-2147483648, 2147483647); migiusiro.setInputLimits(-2147483648, 2147483647); hidarimae.setInputLimits(-2147483648, 2147483647); hidariusiro.setInputLimits(-2147483648, 2147483647); //制御量の最小、最大 //右旋回(目標に達してない) if((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < target)) { migimae.setOutputLimits(0x84, 0xFF); migiusiro.setOutputLimits(0x84, 0xFF); hidarimae.setOutputLimits(0x00, 0x7B); hidariusiro.setOutputLimits(0x00, 0x7B); } //左旋回(目標より行き過ぎ) else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) { migimae.setOutputLimits(0x00, 0x7B); migiusiro.setOutputLimits(0x00, 0x7B); hidarimae.setOutputLimits(0x84, 0xFF); hidariusiro.setOutputLimits(0x84, 0xFF); } //よくわからんやつ migimae.setMode(AUTO_MODE); migiusiro.setMode(AUTO_MODE); hidarimae.setMode(AUTO_MODE); hidariusiro.setMode(AUTO_MODE); //目標値 migimae.setSetPoint(abs(target)); migiusiro.setSetPoint(abs(target)); hidarimae.setSetPoint(abs(target)); hidariusiro.setSetPoint(abs(target)); //センサ出力 migimae.setProcessValue(abs(x_pulse1)); migiusiro.setProcessValue(abs(y_pulse1)); hidarimae.setProcessValue(abs(y_pulse2)); hidariusiro.setProcessValue(abs(x_pulse2)); //制御量(計算結果) migimae_data[0] = migimae.compute(); migiusiro_data[0] = migiusiro.compute(); hidarimae_data[0] = hidarimae.compute(); hidariusiro_data[0] = hidariusiro.compute(); //制御量をPWM値に変換 //右旋回(目標に達してない) if((abs(x_pulse1) < abs(target)) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < abs(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((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_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 { 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); } }