Dears, Can you help please? I've made a code to control a robot-car using an accellrometer in my smartphone (with RoboRemo App) and everything worked fine. But when I added a few lines of code to control in addition one servo, then right wheels stoped working correctly - they turn only in maximum vertical psoition of the smartphone.
Here is the robot: LINK
The scheme: LINK
The source of power for the servo - I attached a scheme
And the code:
#include <Servo.h>
Servo serwomechanizm;
int ENA=10; //adjusting speed - right wheels
int ENB=5; //adjusting speed - left wheels
int IN1=2; // defining pin2 left rear
int IN2=4; // defining pin4 left front
int IN3=7; // defining pin7 right rear
int IN4=8; // defining pin8 right front
int PWM_A=0; //Speed R wheels
int PWM_B=0; //Speed L wheels
int factor;
int Iwart;
char* znak;
char* Twart;
int iX;
int iY;
int iS;
const byte numChars = 6;
char receivedChars[numChars]; // an array to store the received data
boolean newData = false;
void setup() {
serwomechanizm.attach(3);
pinMode(ENA,OUTPUT); // pin10 (PWM)
pinMode(ENB,OUTPUT); // pin11 (PWM)
pinMode(IN1,OUTPUT); // pin2
pinMode(IN2,OUTPUT); // pin4
pinMode(IN3,OUTPUT); // pin7
pinMode(IN4,OUTPUT); // pin8
Serial.begin(9600);
}
void loop() {
recvWithEndMarker();
if (newData == true)
{parseData();
sprawdzenie();
newData = false;}
}
void recvWithEndMarker()
{
static byte ndx = 0;
char endMarker = '\n';
char rc;
while (Serial.available() > 0 && newData == false)
{
rc = Serial.read();
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
ndx = 0;
newData = true;
}
}
}
void parseData()
{
znak = strtok(receivedChars,","); // get the first part - the string
Twart = strtok(NULL, ","); // this continues where the previous call left off
Iwart = atoi(Twart); // convert this part to an integer
if (znak[0]=='X')
iX = Iwart;
if (znak[0]=='Y')
iY = Iwart;
if (znak[0]=='S')
iS = Iwart;
}
void sprawdzenie()
{
if (znak[0]=='S')
{
serwomechanizm.write(iS);
}
if (iY<118 && iX>117 && iX<139) //MOVE STRAIGHT
{
PWM_A = map(iY,117,0,50,255);
PWM_B = map(iY,117,0,50,255);
przod();
}
if (iY>138 && iX>117 && iX<139) //MOVE BACK
{
PWM_A = map(iY,139,255,50,255);
PWM_B = map(iY,139,255,50,255);
tyl();
}
if (iY>117 && iY<139 && iX>117 && iX<139) //STOP
{
PWM_A = 0;
PWM_B = 0;
zatrz();
}
if (iY<118 && iX<118) //STARIGHT RIGHT
{
factor = map(iX,117,0,100,0);
PWM_A = (map(iY,117,0,50,255))*factor/100;
PWM_B = map(iY,117,0,50,255);
przod();
}
if (iY<118 && iX>138) //STARIGHT LEFT
{
factor = map(iX,139,255,100,0);
PWM_A = map(iY,117,0,50,255);
PWM_B = (map(iY,117,0,50,255))*factor/100;
przod();
}
if (iY>138 && iX<118) //BACK RIGHT
{
factor = map(iX,117,0,100,0);
PWM_A = (map(iY,139,255,50,255))*factor/100;
PWM_B = map(iY,139,255,50,255);
tyl();
}
if (iY>138 && iX>138) //BACK LEFT
{
factor = map(iX,139,255,100,0);
PWM_A = map(iY,139,255,50,255);
PWM_B = (map(iY,139,255,50,255))*factor/100;
tyl();
}
}
void przod() //straight
{
analogWrite(ENA,PWM_A);
analogWrite(ENB,PWM_B);
digitalWrite(IN1,0);
digitalWrite(IN2,1);
digitalWrite(IN3,0);
digitalWrite(IN4,1);
}
void tyl() //back
{
analogWrite(ENA,PWM_A);
analogWrite(ENB,PWM_B);
digitalWrite(IN1,1);
digitalWrite(IN2,0);
digitalWrite(IN3,1);
digitalWrite(IN4,0);
}
void prawo() //right
{
analogWrite(ENA,PWM_A);
analogWrite(ENB,PWM_B);
digitalWrite(IN1,1);
digitalWrite(IN2,0);
digitalWrite(IN3,0);
digitalWrite(IN4,1);
}
void lewo() //left
{
analogWrite(ENA,PWM_A);
analogWrite(ENB,PWM_B);
digitalWrite(IN1,0);
digitalWrite(IN2,1);
digitalWrite(IN3,1);
digitalWrite(IN4,0);
}
void zatrz() //stop
{
analogWrite(ENA,PWM_A);
analogWrite(ENB,PWM_B);
digitalWrite(IN1,1);
digitalWrite(IN2,1);
digitalWrite(IN3,1);
digitalWrite(IN4,1);
}
