Code.c

ÿþ#include <SoftwareSerial.h>

#include "DualVNH5019MotorShield.h"

DualVNH5019MotorShield md;

int command = 0;  //incoming serial data

int led = 11;

int ledPin = 13;

int buttonpin = 5;

SoftwareSerial bluetooth(A2, A3);

int cmd;

#define SERVO_PIN 3

uint16_t volatile servoTime = 0;

uint16_t volatile servoHighTime = 3000;

boolean volatile servoHigh = false;

void setup() {

  bluetooth.begin(9600);

  Serial.begin(9600); // set up Serial library at 9600 bps - this is the speed the serial interface will work all

  pinMode(led, OUTPUT); //connect here the blue LED

  pinMode (ledPin, OUTPUT);

  pinMode (buttonpin, OUTPUT);

  md.init();

  servoInit();

}

// This ISR runs after Timer 2 reaches OCR2A and resets.

// In this ISR, we set OCR2A in order to schedule when the next

// interrupt will happen.

// Generally we will set OCR2A to 255 so that we have an

// interrupt every 128 us, but the first two interrupt intervals

// after the rising edge will be smaller so we can achieve

// the desired pulse width.

ISR(TIMER2_COMPA_vect)

{

  // The time that passed since the last interrupt is OCR2A + 1

  // because the timer value will equal OCR2A before going to 0.

  servoTime += OCR2A + 1;

  static uint16_t highTimeCopy = 3000;

  static uint8_t interruptCount = 0;

  if(servoHigh)

  {

    if(++interruptCount == 2)

    {

      OCR2A = 255;

    }

    // The servo pin is currently high.

    // Check to see if is time for a falling edge.

    // Note: We could == instead of >=.

    if(servoTime >= highTimeCopy)

    {

      // The pin has been high enough, so do a falling edge.

      digitalWrite(SERVO_PIN, LOW);

      servoHigh = false;

      interruptCount = 0;

    }

  } 

  else

  {

    // The servo pin is currently low.

    if(servoTime >= 40000)

    {

      // We've hit the end of the period (20 ms),

      // so do a rising edge.

      highTimeCopy = servoHighTime;

      digitalWrite(SERVO_PIN, HIGH);

      servoHigh = true;

      servoTime = 0;

      interruptCount = 0;

      OCR2A = ((highTimeCopy % 256) + 256)/2 - 1;

    }

  }

}

void servoInit()

{

  digitalWrite(SERVO_PIN, LOW);

  pinMode(SERVO_PIN, OUTPUT);

  // Turn on CTC mode.  Timer 2 will count up to OCR2A, then

  // reset to 0 and cause an interrupt.

  TCCR2A = (1 << WGM21);

  // Set a 1:8 prescaler.  This gives us 0.5us resolution.

  TCCR2B = (1 << CS21);

  // Put the timer in a good default state.

  TCNT2 = 0;

  OCR2A = 255;

  TIMSK2 |= (1 << OCIE2A);  // Enable timer compare interrupt.

  sei();   // Enable interrupts.

}

void servoSetPosition(uint16_t highTimeMicroseconds)

{

  TIMSK2 &= ~(1 << OCIE2A); // disable timer compare interrupt

  servoHighTime = highTimeMicroseconds * 2;

  TIMSK2 |= (1 << OCIE2A); // enable timer compare interrupt

}

void loop() {

  if (bluetooth.available()) {

    cmd = bluetooth.read();

    Serial.println(cmd);

    Serial.write(cmd);

    switch (cmd)

    {