//balancing skateboard code for arduino - 092307 - thanks to anyone who's code samples I've used
//info at chriscoleman.com/stuff

#define imuRefPin        0        //voltage ref - not using right now
#define tiltPin          2
#define ratePin          3
#define safety           2        //safety switch
#define disable          7        //output to motor controller
#define AHI              8        //output to motor controller
#define ccwPin           9        //PWM output to motor controller
#define BHI             10        //output to motor controller
#define cwPin           11        //PWM output to motor controller
#define filterSamples   13        //filterSamples should  be an odd number, no smaller than 3
#define MAXOUTPUT      220        //speed limit

  float Kp = 8;
  float Kd = 3;
  float Ki = .15;
  int Ko = 4;                           // Output Scale
  int Perror,PrevErr,Ierror = 0;
  int output,doingOutput = 0;
  int proportional,integral,derivative = 0;

int sensSmoothArray1 [filterSamples];   // array for holding raw sensor values for sensor1 
int sensSmoothArray2 [filterSamples];   // array for holding raw sensor values for sensor2 

int rawTilt, smoothTilt;                // variables for sensor1 data
int rawRate, smoothRate;                // variables for sensor2 data

int tiltOffset = 0;
int rateOffset = 0;
int readPots = 0;

/*******************************************************************************
00000000000000000000000000000000000000000000000000000000000000000000000000000000
*******************************************************************************/

void setup() {
  pinMode(tiltPin, INPUT);
  pinMode(ratePin, INPUT);
  pinMode(safety, INPUT);
  pinMode(disable, OUTPUT);
  digitalWrite( disable, HIGH );    // disable the motor controller until rider/program are ready
  pinMode(AHI, OUTPUT);
  digitalWrite( AHI, HIGH );
  pinMode(BHI, OUTPUT);
  digitalWrite( BHI, HIGH );
  Serial.begin(9600);
}

/*******************************************************************************
00000000000000000000000000000000000000000000000000000000000000000000000000000000
*******************************************************************************/

void loop() {
  
  if ( readPots == 0 ) {
    delay(150);                //readings seemed to need a delay to stabilize
    while ( readPots < 30 ) {
      rawRate = analogRead(ratePin);
      smoothRate = digitalSmooth(rawRate, sensSmoothArray2);
      rateOffset = smoothRate;
      rawTilt = analogRead(tiltPin);
      smoothTilt = digitalSmooth(rawTilt, sensSmoothArray1);
      tiltOffset = smoothTilt + 12;
      readPots++;
    }
  }

  rawTilt = (analogRead(tiltPin) - tiltOffset);           // read accelerometer and subtract offset to get a 0 center value
  smoothTilt = digitalSmooth(rawTilt, sensSmoothArray1);  // every sensor you use with digitalSmooth needs its own array

  rawRate = (analogRead(ratePin) - rateOffset);           // read gyro and subtract offset to get a 0 center value
  smoothRate = digitalSmooth(rawRate, sensSmoothArray2);  // every sensor you use with digitalSmooth needs its own array
  
  Perror = ( smoothTilt * 6 ) + (smoothRate * 2);      //just made up these multiples and it works so I haven't gone
                                                       //back to figure out if I can improve this. I'm using tilt for
                                                       //the majority of error, but tilt only has a resolution of 24 
                                                       //steps from one extreme to the other.

  DoPID();
  
  if ( digitalRead(safety) == HIGH ) {       // safety switch engaged (on)
    if ( doingOutput == 1 ) {
      digitalWrite ( disable, LOW );           // enable controller
      DoOutput();                              // send output
    } else {
      if ( smoothTilt > 0 ) {
        doingOutput = 1;
        Ierror = 0;
        digitalWrite ( disable, LOW );           // enable controller
        DoOutput();                              // send output
      } else { digitalWrite ( disable, HIGH ); } // wait for rider to mount/remount
    }
  } else { 
    digitalWrite ( disable, HIGH );          // disable the motor controller until rider/program are ready
    doingOutput = 0;                         // reset output flag - wait for rider to mount/remount
  }
  
//  int val = analogRead(imuRefPin);
  int val = Kp;
  int val2 = Kd;
    Serial.print(val);
    Serial.print("\t");
    Serial.print(val2);
    Serial.print("\t");
    Serial.print(smoothTilt);
    Serial.print("\t");
    Serial.print(smoothRate);
    Serial.print("\t");
    Serial.print(output);
    Serial.print("\t");
    Serial.print(Ierror);
    Serial.print("\t");
    Serial.print(Perror);
    Serial.println();
}

/*******************************************************************************
00000000000000000000000000000000000000000000000000000000000000000000000000000000
this loop adapted from http://www.barello.net/Papers/Motion_Control/index.htm
*******************************************************************************/

void DoPID() {
  
    proportional = Kp * Perror;

    integral = Ki * Ierror;

    derivative = Kd * (Perror - PrevErr);
    
    PrevErr = Perror;
    Ierror += (Perror * 2);
    
      if ( Ierror > 3000 ) { Ierror = 3000; }    //I'm using this to ramp output/speed based on time
      else if ( Ierror < -3000 ) { Ierror = -3000; }
      
    output = (proportional + integral - derivative) / Ko;
    
      if (output >= MAXOUTPUT)          // verify value is not out of range
        { output = MAXOUTPUT; }
      else if (output <= -MAXOUTPUT)
        { output = -MAXOUTPUT; }
        
}

/*******************************************************************************
00000000000000000000000000000000000000000000000000000000000000000000000000000000
*******************************************************************************/

void DoOutput() {

    if ( output < 0 ) {
      output = abs(output); 
      analogWrite( ccwPin, 0 );
      analogWrite( cwPin, output );
    } else { 
      analogWrite( cwPin, 0 );
      analogWrite( ccwPin, output );
    }

} 

/*******************************************************************************
00000000000000000000000000000000000000000000000000000000000000000000000000000000
digital smooth function from somewhere on the arduino website or samples - sorry 
I don't know exactly where I found it
*******************************************************************************/

int digitalSmooth(int rawIn, int *sensSmoothArray){     // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int j, k, temp, top, bottom;
  long total;
  static int i;
 // static int raw[filterSamples];
  static int sorted[filterSamples];
  boolean done;

  i = (i + 1) % filterSamples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn;                 // input new data into the oldest slot

  for (j=0; j<filterSamples; j++){     // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (filterSamples - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }

  // throw out top and bottom 15% of samples - limit to throw out at least one from top and bottom
  bottom = max(((filterSamples * 15)  / 100), 1); 
  top = min((((filterSamples * 85) / 100) + 1  ), (filterSamples - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
  }

  return total / k;    // divide by number of samples
}