Bullduino source code

Thought I would post the sketch for the bullduino.  You can see it’s tailored to my application, but I could easily change it to be a serially controlled anything that needs a pair of servos and a few digital outputs.  You can see that it’s not a complicated sketch, but I did hijack timer1 for pid interrupts:

 

/* coaster.  The bullduino sketch created for #rbcreation 2012, and for my kids!
Author:  Dave Barrett
*/

#define ledPin 13
#define FAN 2
#define Kp 6    // these are the factors used for pid tuning.  A little ziegler-nichols followed by trial and error
#define Kd 10
#define DEADZONE 3

#define FORWARD   350
#define BACK      720
#define LEFT      670
#define RIGHT     350

#define TOP_SPEED  255

bool escapeflag=false, PIDflag1=false, PIDflag2=false, timerflag=false;
char incomingByte[5];   // for incoming serial data
int total=0, maxspeed=250;
int error=0, previous_error=0, previous_error2=0, servogoal_1 = 510, servopos_1=0, topspeed_1 = 255, servogoal_2 = 512, servopos_2=0, topspeed_2 = 255;
int currentspeed=0, command_index=0, checksum=0;
bool directionflag, last_directionflag;


#define servo1feedback 0
#define servo2feedback 1

void setup()
{
  pinMode(ledPin, OUTPUT);
  pinMode(FAN, OUTPUT);
  digitalWrite(FAN, LOW);
  pinMode(7, OUTPUT);
  digitalWrite(7,HIGH);
  pinMode(8, OUTPUT);
  digitalWrite(8,HIGH);
 
  // initialize timer1
  noInterrupts();           // disable all interrupts
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;

  OCR1A = 625;            // compare match register 16MHz/256/100Hz
  TCCR1B |= (1 << WGM12);   // CTC mode
  TCCR1B |= (1 << CS12);    // 256 prescaler
  TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt
  interrupts();             // enable all interrupts
  Serial.begin(9600);     // opens serial port, sets data rate to 9600 bps
}

void go(int howfast)
{
      if (howfast >= 0)
      {
        if (howfast > maxspeed)
          howfast = maxspeed;
        analogWrite (3, 0);
        analogWrite (11,howfast);
      }
      else
      {
        if (howfast < -maxspeed)
          howfast = -maxspeed;
        analogWrite (11,0);
        analogWrite (3, -howfast);
      }
}

void go2(int howfast)
{
      if (howfast >= 0)
      {
        if (howfast > maxspeed)
          howfast = maxspeed;
        analogWrite (5, 0);
        analogWrite (6,howfast);
      }
      else
      {
        if (howfast < -maxspeed)
          howfast = -maxspeed;
        analogWrite (6,0);
        analogWrite (5, -howfast);
      }
}
        
int doPid(void)
{
  int Dterm,Pterm;
  error=(servopos_1 - servogoal_1);
  Dterm= Kd * (previous_error - error);
  Pterm = Kp * error;
  previous_error = error;
  if (abs(error) > DEADZONE)
    return Pterm - Dterm;
  else
    return 0;
}

int doPid2(void)
{
  int Dterm,Pterm;
  error=(servopos_2 - servogoal_2);
  Dterm= Kd * (previous_error2 - error);
  Pterm = Kp * error;
  previous_error2 = error;
  if (abs(error) > DEADZONE)
    return Pterm - Dterm;
  else
    return 0;
}

ISR(TIMER1_COMPA_vect)          // timer compare interrupt service routine - also the pid loop flag
{
  if (timerflag)
  {
      servopos_2=analogRead(servo2feedback);
      timerflag=false;
      //digitalWrite(ledPin, digitalRead(ledPin) ^ 1);   // toggle LED pin
      analogRead(servo1feedback); //prep for other servo feedback
      PIDflag2=true;
  }
  else
  {
      servopos_1=analogRead(servo1feedback);
      timerflag=true;
      //digitalWrite(ledPin, 0);   // toggle LED pin
      analogRead(servo2feedback); //prep for other servo feedback
      PIDflag1=true;
  }
}

void loop()
{
  int i;
  char command;
 
  if (PIDflag1)
  {
    PIDflag1=false;
    currentspeed=doPid();
    go(currentspeed);
    //Serial.print(servopos_1,DEC);
    //Serial.println(servogoal_1,DEC);
  }
 
  if (PIDflag2)
  {
    PIDflag2=false;
    currentspeed=doPid2();
    go2(currentspeed);
  }
 
  if (Serial.available())
  {
      command=Serial.read();
      if (command==126)
        command_index=0;
      else
      {
        incomingByte[command_index]=command;
        command_index++;
        if (command_index > 4) //then we should know where to go
        {
          checksum=0;
          for (i=0; i < 4; i++) //just make sure the checksum matches
          {
            checksum+=incomingByte[i];
          }
          if (checksum == incomingByte[4])  //ding ding ding we have a winner!
          {
            //digitalWrite(ledPin, digitalRead(ledPin) ^ 1);   // toggle LED pin
            if (incomingByte[0] == 0)  // then we're pitch
            {
              servogoal_2 = (BACK - (incomingByte[1] * ((BACK - FORWARD) / 14)));
            }
            else  //we're roll
            {
              servogoal_1 = (LEFT - (incomingByte[1] * ((LEFT - RIGHT) / 14)));
            }
            
            maxspeed = TOP_SPEED - ((3 - incomingByte[2]) * 40);
            if (!incomingByte[3])
              digitalWrite(FAN, HIGH);
            else
              digitalWrite(FAN, LOW);
          }
        }
        
              
      }
  }
 
}


Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s