#define Motor_L1_PIN 6 // 控制小車左前方電機前進 Control the motor on the left front of the car
#define Motor_L2_PIN 8 // 控制小車左后方電機前進 Control car left rear motor forward
#define Motor_R1_PIN 7 // 控制小車右前方電機前進 Control the right front motor of the car to move forward
#define Motor_R2_PIN 9 // 控制小車右后方電機前進 Control car right rear motor forward
typedef struct {
long myversion;
long dj_record_num;
byte pre_cmd[PRE_CMD_SIZE+1];
int dj_bias_pwm[SERVO_NUM+1];
}eeprom_info_t;
eeprom_info_t eeprom_info;
Servo myservo[SERVO_NUM]; //創建一個舵機類
typedef struct { //舵機結構體變量聲明
unsigned int aim = 1500; //舵機目標值
float cur = 1500.0; //舵機當前值
unsigned int time1 = 1000; //舵機執行時間
float inc= 0.0; //舵機值增量，以20ms為周期
}duoji_struct;
duoji_struct servo_do[SERVO_NUM]; //用結構體變量聲明一個舵機變量組
void AI_parse_cmd() {
if (String(uart_receive_str_bak).length()) {
if (String(uart_receive_str_bak).equals(String("$SMODE0!"))) {
Serial.println("switch AI mode!");
int R_count=0;//左轉路口
int L_count=0;//右轉路口
int ALL_count=0;//檢測到所有黑線
int block=0;
int UR=0;//超聲波傳感器是否識別到物塊
int clo=1;//顏色分別1：黑色，假設在第一個格子 2：白色，假設在第二個格子 3：紅色，假設在第三個格子 4：黃色，假設在第4個格子 5：綠色，假設在第四個格子
//x1-x8 從左往右數
uint8_t x1,x2,x3,x4,x5,x6,x7,x8;
void Motor_init() {
Wire.begin(); // 初始化I2C通訊 Initialize I2C communication
delay(1000); // 如果小車功能異常，可以增加這個延時 If the function is abnormal, you can increase the delay
setCarMove(STOP); // 設置小車停止狀態 Set the car to stop state
}
/**
* @brief 設置單個電機速度 Setting the Motor Speed
* @param motor_forward_pin: 控制電機前進引腳 Control the motor forward pin
* @param motor_backward_pin: 控制電機后退引腳 Control the motor backward pin
* @param motor_speed: 設置電機速度 Setting the Motor Speed
* @retval 無 None
*/
void setMotorSpeed(int motor_speed_L, int motor_speed_R) {
motor_speed_L = map(motor_speed_L, -120, 120, -1200, 1200);
motor_speed_R = map(motor_speed_R, -120, 120, -1200, 1200);
car_run(motor_speed_L,motor_speed_R);
}
/**
* @brief 設置小車運動方式和速度 Set the car movement mode and speed
* @param Movement: 小車運動方式 Car movement
* @param Speed: 小車運動速度 Car speed
* @retval 無 None
*/
void setCarMove(uint8_t Movement) {
switch (Movement) {
case STOP:
car_run(0, 0);
break;
case FORWARD:
car_run(800, 800);
break;
case BACKWARD:
car_run(-800, -800);
break;
case LEFT:
car_run(-800, 800);
break;
case RIGHT:
car_run(800, -800);
break;
default:
car_run(0, 0);
break;
}
}
int myabs(int a)
{
if(a<0)
return -a;
return a;
}
int myignore_speed(int speed)
{
if(speed == 0)
return 0;