What's wrong with my this code

Hello all forum’s member,
I would like to ask, what’s wrong with this code?

#include <Servo.h>  

// Two motors, a servo, and an IR range sensor.
Servo sensorServo, centerServo;
const int irPin = A0; // analog pin 0

// Timing. I don't like using delay.
unsigned long tCnt = 0;
unsigned long tStart = 0;
unsigned long tDelta = 0;
unsigned long tTurn = 0;
unsigned long tTurnDur = 400;  //turn duration

int state;                  // Current Robot State
int lastState;              // Previous Robot State
int servoPos;               // Position to send servo
int centerPos;              // Center position to send servo
int servoDirection;         // Direction servo is turning
int lastDetectionAngle;     // Position of servo at last IR detect.
int sensorValue = 0;        // sensor value
const int minDistance = 20; // minimum distance 20 cm

// Constants for state of robot wheel
const int STATE_FORWARD = 1;
const int STATE_TURN_RIGHT = 2;
const int STATE_BACKWARD = 3;
const int STATE_TURN_LEFT = 4;  

// Constants for Servo.
const int DIR_LEFT = 0;
const int DIR_RIGHT = 1;
const int MIN_DEGREE = 40;
const int MAX_DEGREE = 140;  

// Constants for robot wheel.
const int RIGHTSPEED = 255;  //right motor speed
const int LEFTSPEED  = 255;  //left motor speed
int motorR = 5;       //right motor
int dirmotorR = 4;    //motor direction
int motorL = 6;       //left motor
int dirmotorL = 7;    //motor direction

// Using Ultrasonic Range Sensor HC-SR04
#include <NewPing.h>

#define TRIGGER_PIN  2  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     3  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 300 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

 * Initializes everything. Is run once.
void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  sensorServo.attach(9);  // attaches the servo on pin 9
  centerServo.attach(10);  // attaches the servo on pin 10
  tStart = millis();
  lastState = state = STATE_FORWARD;
  servoPos = 90;
  centerPos = 90;
  servoDirection = DIR_RIGHT;
  //state = 0; // Uncomment to have robot not move wheel.

 * Runs continuously.
 * 1. Update Servo.
 * 2. Check IR range sensor.
 * 3. Move Robot

void loop() {
  tDelta = millis() - tStart;
  tCnt += tDelta;
  tStart += tDelta;  

  // Tell the servo to move 2 degrees every 25 ticks.
  if( tCnt > 25 ) {
    tCnt = 0;
    if( servoDirection == DIR_LEFT ) {
      servoPos -= 2;
    } else if( servoDirection == DIR_RIGHT) {
      servoPos += 2;

    // Servo position will be beyond desired angles, turn around.
    if( servoPos >= MAX_DEGREE ) {
      servoDirection = DIR_LEFT;
    } else if( servoPos <= MIN_DEGREE ) {
      servoDirection = DIR_RIGHT;

  // Allows disabling of tracks by setting state to 0.
  if(state == 0) {

  // sensorValue = read_gp2d12_range(irPin); //uncomment this line if using Sharp GP2D12
  int sensorValue = sonar.ping_cm(); // Send out the ping, get the results in centimeters.
  if (sensorValue <= minDistance && sensorValue > 0) {
    lastDetectionAngle = servoPos;
    state = STATE_BACKWARD;
  } else {
    if( state == STATE_BACKWARD ) {
      if( lastDetectionAngle > 105 ) { // right
        state = STATE_TURN_LEFT;
        tTurn = tTurnDur;
      } else if( lastDetectionAngle < 75 ) { // left
        state = STATE_TURN_RIGHT;
        tTurn = tTurnDur;
      } else { // center
        state = STATE_TURN_RIGHT; // for now, turn right by default.
        tTurn = tTurnDur;
    } else if ( state == STATE_TURN_RIGHT || state == STATE_TURN_LEFT ) {
      if (tTurn > tDelta) {
        tTurn -= tDelta;
      if( tTurn <= 10 || tTurn < tDelta ) {
        state = STATE_FORWARD;
    } else {
      state = STATE_FORWARD;


 * Uses the state of the robot to move wheels accordingly
void moveRobot() {  

  // The motors seemed to respond better if they receive a stop before a switch in direction.
  if( state != lastState ) {
    RMotor(0,false); //stopped
    LMotor(0,false); //stopped

  switch( state ) {
      return; // helps test, state 0 = dont move.
    case STATE_FORWARD: {
      RMotor(RIGHTSPEED,true); // turn it on going forward
      LMotor(LEFTSPEED,true);  // turn it on going forward
    case STATE_BACKWARD: {
      RMotor(RIGHTSPEED,false); // turn it on going backward
      LMotor(LEFTSPEED,false);  // turn it on going backward
    case STATE_TURN_RIGHT: {
      RMotor(RIGHTSPEED,false); // turn it on going right
      LMotor(LEFTSPEED,true);   // turn it on going right
    case STATE_TURN_LEFT: {
      RMotor(RIGHTSPEED,true); // turn it on going left
      LMotor(LEFTSPEED,false); // turn it on going left

  lastState = state;

// right motor
void RMotor(int pwmspeed, boolean forward) {
  analogWrite(motorR, pwmspeed);
  if (forward) {
    digitalWrite(dirmotorR, HIGH);
  else {
    digitalWrite(dirmotorR, LOW);

//left motor
void LMotor(int pwmspeed, boolean forward) {
  analogWrite(motorL, pwmspeed);
  if (forward) {
    digitalWrite(dirmotorL, HIGH);
  else {
    digitalWrite(dirmotorL, LOW);

// Sharp GP2D12 read distance function
// Uncomment the code below if using Sharp GP2D12 for range sensor
/*float read_gp2d12_range(byte pin)
  int tmp;

  tmp = analogRead(pin);
  if (tmp < 3)
    return -1; // invalid value
  return (6787.0 /((float)tmp - 3.0)) - 4.0; // output in cm
} */

/* EOF */

I have project to build robot has capability run avoid restriction. my problem, the robot only run backward whereas no restriction in front of the robot. please help me…

 Serial.begin(9600);           // set up Serial library at 9600 bps

What do your debug prints tell you?

  tDelta = millis() - tStart;
  tCnt += tDelta;
  tStart += tDelta;

You should not be adding time values.

moveRobot() is called on every pass through loop. Most times, it should do nothing. That is not the case now.

  if (sensorValue <= minDistance && sensorValue > 0) {

This is OK, but looks backwards. Describing what this statement does, in English, goes like:
“If the sensor value is less than or equal to the minimum distance and is greater than zero, do something.”

If you were to express the requirement before writing code, it would go like:
“If the sensor value is greater than zero and less than or equal to the minimum distance, do something.”

While the code is correct, one needs to think about it for a while.

  if(sensorValue > 0 && sensorValue <= minDistance)

does not require as much thought.

You should not be adding time values.

Please explain why this should not be done? The integer math with overflow does work in both directions.

There was a long thread a while ago that explained why addition doesn't work, but subtraction does. I'm not going to reiterate the whole thread.

You seem to not have understood it correctly. It doesn’t work if you do something like this:

uint32_t endMark = millis() + 300;
// ...
if (millis() < endMark) {
  // do something

But this doesn’t mean you shouldn’t add time values generally. If you do something like:

uint32_t savedMillis = 0;
uint32_t deltaT;
if ((deltaT = millis() - savedMillis) > 400) {
  savedMillis += deltaT;
  // do something

This is absolutely OK and won’t give you any problems. The problem is not the addition or subtraction but an addition often calculates a future value and with that value a comparison is only valid if you take care for the possible overruns. That’s why it’s often advisable to favor subtraction over addition in the case of time values.
The problem here is that the OP has used the time values correctly, so the addition is not a problem in this case, so he shouldn’t be corrected in my opinion.

After I traced more, I found the wrongness point.
Seem, because call moveRobot() procedure in loop program result problem.
But I don’t know how to fix it.

What is others solution to make the robot run while ultrasonic sense. Because if I try to erase the moveRobot() procedure from the loop, the ultrasonic sensor can work well. this case I know if I print the result of sensing.

Seem, because call moveRobot() procedure in loop program result problem.

Can you give us a bit more detail? What exactly is your problem? What do you expect from your program, what output do you get?

what's wrong with code?

Aside from the poor formatting? It's incomplete. How are we supposed to know what is wrong when you don't post ALL of your code.

Without it, all I can suggest is that you look at line 38. Doesn't look right to me.

The complete program I have included above.
My problem : this program can’t control my hardware(robot) run well, this program will return 0-1 from sensing distance where the value of sensing I patch in variable int sensorValue. If I print sensorvalue, it will result 0 or 1 whereas no obstacle in front of the sensor. So if this program I upload to the hardware of Arduino, the hardware will move backward always although no obstacle in front of the hardware.
My expectation : I want this program can control hardware run forward if no obstacle but I it founds obstacle, it will run to avoid the obstacle.

here is the picture of the hardware :



Are you really sure you're using an ultrasonic HC-SR04 distance sensor? Mine look very different. From the picture I would guess it's an IR distance sensor and not an ultrasonic one. But you cannot use the NewPing library for IR distance sensors.

The picture very clearly shows a Sharp optical sensor - it is even labelled as such.

I am sorry for your inconvenient, I am really sure use an ultrasonic HC-SR04 distance sensor. This sensor can work well if I omit moveRobot() procedure from loop program. Whereas I should use the moveRobot() procedure to control hardware move anywhere while sensing. Yeah, in the picture shows no an ultrasonic HC-SR04 distance sensor. Because I change the distance sensor hardware become ultrasonic HC-SR04 distance sensor.

Any others solution to solve my problem, I very need the solution because this is my project from my school.

It really isn't clear what your problem is. Beyond calling Serial.begin() in setup(), you never call any other Serial methods, like Serial.print() to tell you what is happening. Why not?

In moveRobot(), you should be printing the state. It may not be what you think it is. If it is what you expect it to be, and the function does not cause the robot to react correctly, there is one area of the code to look at. If the state is NOT what you expect, there is another area of the code to look at.

If you keep dividing the code into two parts - this works and this might not, pretty soon you have conquered the problem.

I don't like your code format. I prefer the { on new lines, so that they line up with the matching } (or they do if you use Tools + Auto Format). That makes it easier to see the structure of the code, in my opinion.

And, you have a lot of comments that state the blindingly obvious. Delete them. Then, add some comments that describe what the code is supposed to do. We can then decide whether the code does what you say it should do, or not.

I am really sure use an ultrasonic HC-SR04 distance sensor.

In this case post a picture of your real hardware, take the picture from an angle so we can see the hardware wiring, at least as far as this is possible.

So you define a bunch of upper case constants to represent forward and backwards and left and right, and then you declare variable to presumably represent the current motor state, and you assign strange values like 7 to it. Why ?