Управляем машинкой через Bluetooth с планшета или телефона под Android (с помощью приложения 4joy-Remote Joystick). Машинка может ездить вперед и поворачивать направо и налево. Но назад не ездить. Помогите исправить Код (Text): #define MIN_speed 0 int PIN_ENA = 9; //левый int PIN_IN4 = 4; //левый мотор назад int PIN_IN3 = 5; //левый мотор вперед int PIN_IN2 = 6; //правый мотор вперед int PIN_IN1 = 7; //правый мотор назад int PIN_ENB = 10; //правый int LED_PIN = 13; float vel,ks,m1,m2; float spl, spr; int quadr = 0; int ver, hor; unsigned char mode; unsigned char oldmode; unsigned char iSpeed; unsigned char rx_buf[8]; unsigned char rxcnt; boolean binv = 0; boolean rx_ok; // Правый мотор void RM_foward() //вперед { digitalWrite(PIN_IN1, LOW); digitalWrite(PIN_IN2, HIGH); } void RM_back() //назад { digitalWrite(PIN_IN2, LOW); digitalWrite(PIN_IN1, HIGH); } // Левый мотор void LM_foward() //вперед { digitalWrite(PIN_IN4, LOW); digitalWrite(PIN_IN3, HIGH); } void LM_back() //назад { digitalWrite(PIN_IN3, LOW); digitalWrite(PIN_IN4, HIGH); } void setup() { Serial.begin(9600); pinMode(PIN_ENB, OUTPUT); pinMode(PIN_IN4, OUTPUT); pinMode(PIN_IN3, OUTPUT); pinMode(PIN_IN2, OUTPUT); pinMode(PIN_IN1, OUTPUT); pinMode(PIN_ENA, OUTPUT); pinMode(LED_PIN, OUTPUT); digitalWrite(PIN_IN1, LOW); digitalWrite(PIN_IN2, LOW); digitalWrite(PIN_IN4, LOW); digitalWrite(PIN_IN3, LOW); digitalWrite(PIN_ENA, LOW); digitalWrite(PIN_ENB, LOW); } //Параметры задания скорости void setspeed() { float sf1; //левый двигатель if (spl>0.0) { LM_foward(); } else { LM_back(); } sf1=abs(spl); sf1=sf1*(255-MIN_speed)+MIN_speed; if (spl==0.0) sf1=0; iSpeed = int(sf1); analogWrite(PIN_ENA, iSpeed); //правый двигатель if (spr>0.0) { RM_foward(); } else { RM_back(); } sf1=abs(spr); sf1=sf1*(255-MIN_speed)+MIN_speed; if (spr==0.0) sf1=0; iSpeed = int(sf1); analogWrite(PIN_ENB, iSpeed); } void loop() { if (Serial.available() == 7) { rxcnt=0; digitalWrite(LED_PIN, binv); binv=!binv; while (Serial.available() > 0) { rx_buf[rxcnt] = Serial.read(); rxcnt++; } //Контроль целостности пакета rx_ok=true; if (rx_buf[4]!=4) rx_ok=false; //прочистка буфера if (!rx_ok) { int avi = Serial.available(); Serial.print("err"); while (Serial.available() > 0) { avi = Serial.read(); } } if (rx_ok) { hor = rx_buf[1]; ver = rx_buf[2]; quadr = 0; //Вычисление рабочего квадранта float fhor=float(hor); float fver=float(ver); if (fhor>128.0) { fhor=256.0-fhor; quadr = quadr+1; } if (fver>128.0) { fver=256.0-fver; quadr = quadr+2; } //нормализация скоростей fhor=fhor/127.0; fver=fver/127.0; if (fver>fhor) { m1 = fver; m2 = (1.0-fhor)*fver; } else { m1 = fhor; m2 = (fver-1.0)*fhor; } if ((fver<0.1)&&(fhor<0.1)) { m1=0; m2=0; } //Приведение знаков скоростей по квадрантам if (quadr==0) { spl=-m1; spr=-m2; } if (quadr==1) { spr=-m1; spl=-m2; } if (quadr==2) { spr=m1; spl=m2; } if (quadr==3) { spl=m1; spr=m2; } setspeed(); } } if (Serial.available() > 7) { int avi = Serial.available(); Serial.print("e"); while (Serial.available() > 0) { avi = Serial.read(); } } }
До боли знакомый код. Прога заточена под L298 (Motorshield). Если у вас что-то другое, то может и не работать. Нужно переписывать void setspeed()
