   SWSerial.begin(9600);
   // Initialize serial communications at 9600 bps:
   Serial.begin(19200);
+  Input = 0;
+  Setpoint = 0;
+  //turn the PID on
+  myPID.SetMode(AUTOMATIC);
+  myPID.SetOutputLimits(-128, 128);
 }
 
 void SerialEvent() {

  if (millis()-lastWriteTime>100)
  {
    lastWriteTime = millis();
    Serial.println(alphaSigned);
    if (isSerialControl)
    {
      //Serial.println("S");

void loop()
{
  SerialEvent();
  myPID.Compute();
  int power;
  sensorValue = analogRead(analogInPin);
  setMode();
  //alphaSigned=Output/127.0;
  computeAlphaSigned();
  power = alphaSigned*127;
  ST.motor(1, power);
