   if (doSerialInterface){
      Serial1.begin(115200);
   } //if doSerialInterface
-
-  Serial.println("\nSetup function completed");
 }//End of setup function 
 
 


  // Connect to the PC's serial port via the Arduino programming USB connection. 
  // This used mainly for printing de-bugging information to the PC's serial terminal
  // during testing.
  Serial.begin(115200);



     //Pre-calculate the speeds for different hat-stick values. This moves these
     //calculations out of the main loop, and allows for smoother closed-loop hat-stick motions.
     for (ii=0; ii<128; ii++){
       for (jj=0; jj<4; jj++){         
          //SPEEDMAT[ii][jj]=(ii/127.5)*maxSpeed[jj]; //Plain linear
          SPEEDMAT[ii][jj]=fscale(hatStickThresh, 127.5, 0.04, maxSpeed[jj], ii, curve[jj]); //non-linear mapping
       }  
