From Arduino Uno R2 to R3: Uploaded code doesn't work + serial monitor no output

Hello forumers,

I upgraded my arduino Uno R2 to a R3 recently and followed the installations correctly.
Am using Arduino 1.0.3 version.
The basic codes such as Blink LED works on my new R3, but I get no response from my own code (see below), and serial monitor does not show output. The previous Uno R2 could work fine, but even when I re attached back the R2, does not work as well now. Is there something missing in my code to comply with the new R3? The code is for a balancing scooter, using Arduino Uno, 5DOF IMU SEN-11072 and Sabertooth 2x25. Any help is greatly appreciated.

//************************ Include header

#include <math.h>
#include <SoftwareSerial.h>

//************************ Initialise Sabertooth

#define SABER_TX_PIN 13
#define SABER_RX_PIN 12
#define SABER_BAUDRATE 9600

#define SABER_MOTOR1_FULL_FORWARD 112
#define SABER_MOTOR1_FULL_REVERSE 15
#define SABER_MOTOR2_FULL_FORWARD 240
#define SABER_MOTOR2_FULL_REVERSE 143
#define SABER_ALL_STOP 0

SoftwareSerial SaberSerial = SoftwareSerial (SABER_RX_PIN, SABER_TX_PIN);

void initSabertooth (void)
{
  pinMode (SABER_TX_PIN, OUTPUT);
  SaberSerial.begin(SABER_BAUDRATE);
  delay(2000);
  SaberSerial.print(0);
}

//**********************

//********************** Define Set points (balancing values)

#define X_ACC_REF 332  
#define Z_ACC_REF 336
#define Y_RATE_LR_REF 276 
#define Y_RATE_HR_REF 272 
#define STEERING_REF 260

//******************** Initialise variables

// For Data ouput

int data1; 
int data2;
int data3;
int data4;
  
// For Sensors and Computation  

int start = 0;
int ACC_Angle;
int GYRO_Rate;
int actAngle;
int actAngle_1;
float controlsignal=0;
int steer = 0;
int drive = 0;

int Motor1percent;
int Motor2percent;
int cSpeedVal_Motor1;
int cSpeedVal_Motor2;

int SensorRef[5] = {X_ACC_REF, Z_ACC_REF, Y_RATE_LR_REF, Y_RATE_HR_REF, STEERING_REF};
int SensorVal[5] = {0, 0, 0, 0, 0};

int X_ACC_PIN = A0;
int Z_ACC_PIN = A1;
int Y_RATE_LR_PIN = A2;
int Y_RATE_HR_PIN = A4;
int STEER_PIN = A5;
int DEADMAN_PIN = 9;

// To calculate loop times

int STD_LOOP_TIME  = 9;  
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

int buffer[20];
float deltaAngle = 3.14/51; //Arbitrary angle increment size
float angle = 0;
int amplitude = 100;

void setup()
{
  initSabertooth();
  // Set up input and output pins (only digital)
  //pinMode(DEADMAN_PIN,INPUT);
  Serial.begin(9600);  
  //Serial.begin(115200); 
  
}



void updateSensors() 
{                                         
    long v;
    for(int n=0; n<5; n++) 
    {
      v = 0;
      for(int i=0; i<5; i++) v += analogRead(n);
      SensorVal[n] = v/5 - SensorRef[n];
    }
}

 //*************************Kalman filter module

    float Q_angle  =  0.001; //0.001
    float Q_gyro   =  0.003;  //0.003
    float R_angle  =  0.03;  //0.03

    float x_angle = 0;
    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;	
    float dt, y, S;
    float K_0, K_1;

  float kalmanCalculate(float newAngle, float newRate,int looptime) {
    dt = float(looptime)/100;                                    // XXXXXXX arevoir
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;
    
    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;
    
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
    
    return x_angle;
  }
  
  
  float compCalculate(float newAngle, float newRate,int looptime)
  {
      dt = float(looptime)/100;  
    x_angle = (float)(0.98*(x_angle + (dt*newRate))+ 0.02*(newAngle));
    return x_angle;
  }

 

//*******************Data Plotter

void plot(int data1, int data2, int data3, int data4)
{
  int pktSize;
  
  buffer[0] = 0xCDAB;             //SimPlot packet header. Indicates start of data packet
  buffer[1] = 4*sizeof(int);      //Size of data in bytes. Does not include the header and size fields
  buffer[2] = data1;
  buffer[3] = data2;
  buffer[4] = data3;
  buffer[5] = data4;
    
  pktSize = 2 + 2 + (4*sizeof(int)); //Header bytes + size field bytes + data
  Serial.write((uint8_t * )buffer, pktSize);
}

int getAccAngle() 
{
   return arctan2(-SensorVal[1]*1.33, -SensorVal[0]*1.33) +256;
}

int arctan2(int y, int x) 
{                                                            
   int coeff_1 = 128;                                          
   int coeff_2 = 3*coeff_1;
   float abs_y = abs(y)+1e-10;                                 // prevent 0/0 condition
   float r, angle;
   
   if (x >= 0) 
   {
     r = (x - abs_y) / (x + abs_y);
     angle = coeff_1 - coeff_1 * r;
   }  
   else 
   {
     r = (x + abs_y) / (abs_y - x);
     angle = coeff_2 - coeff_1 * r;
   }
   if (y < 0)      return int(-angle);                           // negate if in quad III or IV
   else            return int(angle);
}


int getGyroRate() 
{
    return int((SensorVal[2]*2.44*-1));   
}


void updatemotor() {
  
  
  
  drive = constrain (controlsignal, -1000, 1000);
  drive = drive*0.6;

  steer = 0.35*(SensorVal[4]-520);

  
   Motor1percent = (signed char) constrain(drive-steer, -100, 100);
   Motor2percent = (signed char) constrain(drive+steer, -100, 100);
 
  cSpeedVal_Motor1 = map(Motor1percent*1.2, -100, 100, SABER_MOTOR1_FULL_REVERSE, SABER_MOTOR1_FULL_FORWARD);
  cSpeedVal_Motor2 = map(Motor2percent*1.2, -100, 100, SABER_MOTOR2_FULL_REVERSE, SABER_MOTOR2_FULL_FORWARD);


  SaberSerial.write(cSpeedVal_Motor1);
  SaberSerial.write(cSpeedVal_Motor2);

Serial.print("ACC_Angle "); //title of what is shown
Serial.print(ACC_Angle); //  
Serial.print(" ");
Serial.print("sensorval4 "); //title of what is shown
Serial.print(SensorVal[4]); // print the steer value 
Serial.print(" ");
Serial.print("steer "); //title of what is shown
Serial.print(steer); // print the steer value 
Serial.print(" ");
Serial.print("ctrlsignal "); 
Serial.print(controlsignal); // print the control signal 
Serial.print(" "); 
Serial.print("cSpeedVal_Motor2 "); 
Serial.print(cSpeedVal_Motor2); // print the Motor2 speed value 
Serial.print(" "); 
Serial.print("M2% "); 
Serial.print(Motor2percent); // print the Motor1 speed value 
Serial.print(" ");
Serial.print("cSpeedVal_Motor1 "); 
Serial.print(cSpeedVal_Motor1); // print the Motor1 speed value 
Serial.print(" ");
Serial.print("M1% "); 
Serial.print(Motor1percent); // print the Motor1 speed value 
Serial.println(" ");

}



void loop()
{
  
  // Start Up sequence
  if(start==0)
  {
  delay(50);
  updateSensors();
  delay(1000);
  if(SensorVal[0]>-22.5&&SensorVal[0]<22.5)
  {
    start=1;
  }
  
 }
  
  
 else
{ 
  
  
  //Normal opertaion  
  
  
  //***************Sensor reading and filtering + Deadman + Steering
  updateSensors();
  
  //get angle and gyro  add 2 more functions

  ACC_Angle = getAccAngle();
  GYRO_Rate = getGyroRate();
  
 //actAngle = kalmanCalculate(ACC_Angle, GYRO_Rate, lastLoopTime); 
  
 actAngle_1 = compCalculate(ACC_Angle, GYRO_Rate, lastLoopTime);

 
  //***************PID and motor drive
  controlsignal = (float)((4.5*actAngle_1) + (0.5*GYRO_Rate)) ;  
  
  updatemotor();
  
  //***************Serial Ouput for graphing
  

  //data1=controlsignal;
  //data2 = actAngle_1;
  //data3 = GYRO_Rate;
  //data4 =0; 
  
  //angle = angle + deltaAngle;
  //plot(data1,data2,data3,data4);
  //delay(10);                  //Need some delay else the program gets swamped with data
  
 
  //***************Loop timing 
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();  
}
}

Moderator edit:
</mark> <mark>[code]</mark> <mark>

</mark> <mark>[/code]</mark> <mark>
tags added.

The basic codes such as Blink LED works on my new R3, but I get no response from my own code (see below), and serial monitor does not show output. The previous Uno R2 could work fine, but even when I re attached back the R2, does not work as well now.

Sounds like a wiring fault to me. What do you mean by “does not work as well now” ?

I don’t think it’s wiring fault. At least the serial monitor should start pumping out data values (it did before I made the board change), but it is a blank. On the Uno R3 itself the TX RX LEDs blink during uploading and holds the light for a while after finished uploading.
TX RX LEDs did not blink when serial monitor is executed.

“R2 does not work as well now”- I mean i returned back to the R2 and uploaded the code again. is as good as dead.

I am powering from USB, and also trying to read the data via usb to the serial monitor inside the environment. ty in advance !

You only have output to the Serial monitor in the updatemotor() function.
That function is only called if SensorValue[0] is between -22.5 and 22.5 set by updateSensors() when checking at start up, which sets start to 1

If the sensors are not wired correctly will updateSensors() return the value(s) that you expect ? If not you may never get out of the start up, never call updatemotor() and never print anything. Try putting some debug Serial.print statements in updateSensors() to make sure that the sensors are returning the values that you expect.