Monday, January 2, 2012

Vexduino Software Using Interrupt Timer

It's been some time since I worked with the Vexduino but I had some time over this winter break.  Here is an update of the code that uses the interrupt timers of the Arduino.  I did some study of PPM output using code from a PPM output joystick project located here.

This new should create a better PPM output.  I'm now working on integrating this with code from cellbots.com so that I can attach an android phone to the bot.


#include <Messenger.h>


#include 

/*
 * Vexduino 2.0 Interface
 * -------------------
 *
 * 12/31/2011
 * Copyleft 2011 Jeremy Espino MD
 * Licensed under the the Apache 2.0 License
 * espinoj@gmail.com
 * vexduino.blogspot.com
 * modified for vexplorer compatibility
 *
 */

#define PPM_OUT_PIN 12     // which pin to output the PPM signal on
#define FRAME_LENGTH 20  // length of PPM frame in ms
#define PULSE_START 300  // pulse start width
#define PULSE_MIN 600    // pulse minimum width in microsecs
#define PULSE_MAX 1700   // pulse max width in microsecs
#define SERVO_MAX 9      // max servo value
#define SERVO_CNTR 5     // servo center value
#define CHANNEL_NUM 8    // number of channels in the PPM 

// A pulse starts with a high signal of fixed width (0.3ms),
// followed by a low signal for the remainder of the pulse.
// Total pulse width is proportional to servo position (.6 to 1.7ms)
// conversion factor for servo to signal pulse width
const int conversionFactor = (PULSE_MAX - PULSE_MIN)/ SERVO_MAX; 

// A frame is a succession of pulses, in order of channels,
// followed by a synchronisation pulse to fill out the frame.
// A frame's total length is fixed (20ms)
int servo[CHANNEL_NUM];         // Values to set for motor speed 0 - 9, 5 is neutral
int channel[CHANNEL_NUM];       // Channel pulse width (duration of pulse minus start, in microseconds)
int mapping[CHANNEL_NUM];       // map of vex "channel" to PPM radio channels

ISR(TIMER1_COMPA_vect) {
    ppmoutput(); // Jump to ppmoutput subroutine
}



Messenger message = Messenger(); 
byte isAllStop = 0;             // flag to set all channels to center
int inputValue;                 // input value from the serial
int value[CHANNEL_NUM];
  
// initialize servo and channel values to center points
void allStop() {
  for (int i = 0; i < CHANNEL_NUM; i = i + 1 ) {
    servo[i] = SERVO_CNTR;
  }
  for (int i = 0; i < CHANNEL_NUM; i = i + 1 ) {
    channel[i] = (PULSE_MAX - PULSE_MIN) / 2;
  }
}

void setupPPM() {
  
  // Setup timer
  TCCR1A = B00110001; // Compare register B used in mode '3'
  TCCR1B = B00010010; // WGM13 and CS11 set to 1
  TCCR1C = B00000000; // All set to 0
  TIMSK1 = B00000010; // Interrupt on compare B
  TIFR1  = B00000010; // Interrupt on compare B
  OCR1A = 20000;      // 20mS PPM output refresh
  OCR1B = 1000;

  // setup mapping channel to pulse sequence
  mapping[6]=0;
  mapping[5]=1;
  mapping[4]=2;
  mapping[3]=4;
  mapping[2]=5;
  mapping[1]=6;

  pinMode(PPM_OUT_PIN, OUTPUT);   
}

void setup() {
  setupPPM();
  
  // Initiate Serial Communication
  Serial.begin(57600); 
  
  allStop();

  Serial.println("\nVexduino Transmitter Interface ready!");
}

void ppmoutput() { // PPM output sub

  // This for loop generates the pulse train
  for (int i = 0; i < CHANNEL_NUM; i = i + 1 ) {
    // Calculate pulse durations from servo positions
    channel[i] = int(servo[i]*conversionFactor) + PULSE_MIN;
    
    digitalWrite(PPM_OUT_PIN, HIGH);   // Initiate pulse start
    delayMicroseconds(PULSE_START);    // Duration of pulse start
    digitalWrite(PPM_OUT_PIN, LOW);    // Stop pulse start
    delayMicroseconds(channel[i]);     // Finish off pulse
  }
  digitalWrite(PPM_OUT_PIN, HIGH);     // Initiate synchronisation pulse
  delayMicroseconds(PULSE_START);      // Duration of start of synchronisation pulse
  digitalWrite(PPM_OUT_PIN, LOW);      // Stop synchronisation pulse start
  
}

void readSerialInput() {
    // read the next set of servo settings from serial input
  while ( Serial.available() ) {

    if ( message.process(Serial.read() ) ){
      int p = 1;
      
      while( message.available() ) {
        inputValue = message.readInt();
        
        // a value of 999 indicates all stop position
        if (inputValue == 999) isAllStop = 1;
        
        servo[mapping[p]] =  inputValue;
        
        // if invalid servo position set to center
        if ( servo[mapping[p]] > SERVO_MAX) servo[mapping[p]] = SERVO_CNTR;

        p = p + 1;
      }
      if (isAllStop == 1) {
        allStop();
        isAllStop = 0;
      }
    }
  }
  
}


void outputCurrentSettings() {
  // show current settings
  // skip vex "channels" 0 and 7 since they are numbered 1 thru 6
  Serial.print("\r");
  for (int i = 1; i < 7; i = i + 1 ) {

    Serial.print(i);
    Serial.print(":");
    Serial.print(servo[mapping[i]]);
    Serial.print(" ");

  }
  Serial.print("  ");
  for(int x=0; x<=CHANNEL_NUM-1; x++)  //Loop to print and clear all the channel readings
  {
      Serial.print(x);
      Serial.print(":");
      Serial.print(value[x]); //Print the value
      Serial.print(" ");
      value[x]=0; //Clear the value after is printed
  }
  Serial.print("   "); //Start a new line
}

void loop() {
  readSerialInput();
  outputCurrentSettings();
  delay(10);
}