Recent Changes - Search:

ENTER NEW DAILY DATA HERE:

PmWiki

pmwiki.org

edit SideBar

December24-MandNsCodeNotebook

This is a page for Mike Sudano and Nick Wong (and anyone else who gets involved in this) to post their code snippets for the revised ElmoSlave and ElmoMaster Arduino code.

To get it started, here's the latest Slave and Master code on the Elmos:

Latest ElmoSlave code as of 12/23/08:

/*
 getting signal from master Xbee. moves step by step.
 AUTHOR: mike  & soyoung
 DATE: Dec 2nd 2008
 PURPOSE:
 1. Perform one movement from a routine, each time a remote (wireless) button is pushed.
 */
int val = 0;
//Motor Output Pins:
int armPwmPin = 10; // L293 pin 1
int armMtrPosPin = 2; // L293 pin 2
int armMtrNegPin = 3; // L293 pin 7
int legPwmPin = 11; // L293 pin 9
int legMtrPosPin = 4; // L293 pin 15
int legMtrNegPin = 5; // L293 pin 10
//Connections to drive motors:
// L293 pin 3 - Arm Motor + (green wire)
// L293 pin 6 - Arm Motor - (white wire)
// L293 pin 14 - Leg Motor + (yellow wire)
// L293 pin 11 - Leg Motor - (brown wire)

//Motor Feedback Pins:
int armSensorBrown = 6; //red wire -20 180+
int armSensorRed = 7; //white wire 0 90~180
int armSensorYellow = 8; //yellow wire 90 90
int armSensorGreen = 9; //grey wire 150 0
int legSensorBlue = 14; //blue wire 135
int legSensorGreen = 15; //green wire 90
int legSensorYellow = 16; //yellow wire 30
int legSensorOrange = 17; //orange wire 0
int legSensorRed = 18; //red wire -30

//Gyroscope Feedback Pins:
int gyroGreen = 13; // supine
int gyroYellow = 19; // upright
int gyroRed = 12; // prone
//brown is ground, and orange is not connected

//Variables:
int forward=1;
int reverse=0;
int stepNumber=0; //keep track of which movement to do next

//For debouncing:
int currentStep;

void setup() {
  Serial.begin(9600);
  //Motor Output Pins:
  pinMode(armPwmPin, OUTPUT); //Set pin 3 as an output
  pinMode(armMtrPosPin, OUTPUT); //Set pin 2 as an output
  pinMode(armMtrNegPin, OUTPUT); //Set pin 4 as an output
  digitalWrite(armPwmPin, LOW); //Disable motor on startup
  digitalWrite(armMtrPosPin, HIGH); //Prepare for clockwise motor rotation
  digitalWrite(armMtrNegPin, LOW); //Prepare for clockwise motor rotation

  pinMode(legPwmPin, OUTPUT); //Set pin 3 as an output
  pinMode(legMtrPosPin, OUTPUT); //Set pin 2 as an output
  pinMode(legMtrNegPin, OUTPUT); //Set pin 4 as an output
  digitalWrite(legPwmPin, LOW); //Disable motor on startup
  digitalWrite(legMtrPosPin, HIGH); //Prepare for clockwise motor rotation
  digitalWrite(legMtrNegPin, LOW); //Prepare for clockwise motor rotation

  //Motor Feedback Pins: for arm
  pinMode(armSensorRed, INPUT); //Set pin 7 as an input
  digitalWrite(armSensorRed, HIGH); //Enable internal pull-up resistor
  pinMode(armSensorBrown, INPUT);  //Set pin 8 as an input
  digitalWrite(armSensorBrown, HIGH);  //Enable internal pull-up resistor
  pinMode(armSensorYellow, INPUT);  //Set pin 9 as an input
  digitalWrite(armSensorYellow, HIGH);  //Enable internal pull-up resistor
  pinMode(armSensorGreen, INPUT);  //Set pin 10 as an input
  digitalWrite(armSensorGreen, HIGH);  //Enable internal pull-up resistor
  //Motor Feedback Pins: for legs
  pinMode(legSensorBlue, INPUT); //Set pin 7 as an input
  digitalWrite(legSensorBlue, HIGH); //Enable internal pull-up resistor
  pinMode(legSensorGreen, INPUT);  //Set pin 8 as an input
  digitalWrite(legSensorGreen, HIGH);  //Enable internal pull-up resistor
  pinMode(legSensorYellow, INPUT);  //Set pin 9 as an input
  digitalWrite(legSensorYellow, HIGH);  //Enable internal pull-up resistor
  pinMode(legSensorOrange, INPUT);  //Set pin 10 as an input
  digitalWrite(legSensorOrange, HIGH);  //Enable internal pull-up resistor
  pinMode(legSensorRed, INPUT);  //Set pin 10 as an input
  digitalWrite(legSensorRed, HIGH);  //Enable internal pull-up resistor

//  Serial.print("Step: ");
//  Serial.println(stepNumber);

}
void loop() {
  //Wait for the button to be pressed. Then execute
  // the current stepNumber, and increment the stepNumber.
  currentStep = stepNumber;
  while (currentStep == stepNumber) {
    stepNumber=waitForButton(stepNumber);
  }

//  Serial.print("...doing stepNumber ");
//  Serial.println(stepNumber);
  doNextStep(stepNumber);

  //    
  //    moveArm(armSensorGreen, forward, 100);
}
int waitForButton(int whichStep) {
  if (Serial.available()) {
    val = Serial.read();
    //val = digitalRead(switchPin);
  //  while(val == 'L'){
    //   val = Serial.read();


  //  val = digitalRead(switchPin);
    if(val == 'L') {
 //     return whichStep;

    whichStep = whichStep+1;
//    Serial.print("Step: ");
//    Serial.println(whichStep);
      }

  //  return whichStep;
  }
  return whichStep;
}
void doNextStep(int whichStep) {
  switch(whichStep) {
  case 1:
    moveArm(armSensorRed, reverse, 150);
    break;
  case 2:
    moveArm(armSensorGreen, forward, 150);
    break;
  case 3:
    moveArm(armSensorRed, reverse, 150);
    break;
  case 4:
    moveArm(armSensorGreen, forward, 150);
    break;
  case 5:
    moveArm(armSensorYellow, reverse, 150);      
    break;
  case 6:
    moveLeg(legSensorGreen,forward, 200);
    break;
  case 7:
    stepNumber = 0;
    moveLeg(legSensorOrange, reverse, 200);
    break;
  }
}
void blink(int whatPin, int howManyTimes, int milliSecs) {
  int i=0;
  for(i=0;i<howManyTimes;i++) {
    digitalWrite(whatPin, HIGH);
    delay(milliSecs/2);
    digitalWrite(whatPin, LOW);
    delay(milliSecs/2);
  }
}
void moveArm(int armSensorColor, int whichWay, int howFast) {
  //MOVE MOTOR IN DIRECTION OF "whichWay" (EITHER FORWARD OR REVERSE)
  int positivePin = whichWay;
  int negativePin = ~whichWay & 1;
  digitalWrite(armMtrPosPin, positivePin); 
  digitalWrite(armMtrNegPin, negativePin);
  analogWrite(armPwmPin,howFast); // Turn motor on at speed "howFast"
  //WATCH FOR WHEN THE MOTOR REACHES THE NEXT TARGET SENSOR
  int sensor = 1;
  while(sensor==HIGH) {
    sensor = digitalRead(armSensorColor);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(armPwmPin,0);
}
void moveLeg(int legSensorColor, int whichWay, int howFast) {
  //MOVE MOTOR IN DIRECTION OF "whichWay" (EITHER FORWARD OR REVERSE)
  int positivePin = whichWay;
  int negativePin = ~whichWay & 1;
  digitalWrite(legMtrPosPin, positivePin); 
  digitalWrite(legMtrNegPin, negativePin);
  analogWrite(legPwmPin,howFast); // Turn motor on at speed "howFast"
  //WATCH FOR WHEN THE MOTOR REACHES THE NEXT TARGET SENSOR
  int sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(legSensorColor);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(legPwmPin,0);
}

Latest ElmoMaster code as of 12/23/08:

//soyoung Dec 1st 2008
//Xbee sending switch
int switchPin = 2;
int previousState=0;
int nowState=0;
int stateChange=0;
int ledPin=13; //to double check that code is good

void setup() {
  pinMode(switchPin, INPUT);
  digitalWrite(switchPin, HIGH); //enable internal pull-up resistor
  pinMode(ledPin, OUTPUT);
  Serial.begin(9600);
}
void loop(){
state();
  if (stateChange == 1 && nowState == 1){
    Serial.println('H');
    digitalWrite(ledPin,HIGH);
    previousState=nowState;
    state();
  }
  else if (stateChange == 1 && nowState == 0){
    Serial.println('L');
    digitalWrite(ledPin,LOW);
    previousState=nowState;
    state();
  }
}
void state(){
  nowState=digitalRead(switchPin);
  if (nowState != previousState){
    stateChange=1;
  }
  else {
    stateChange=0;
  }
}

'multiPrint' function to prevent message loss -- 12/28/08

void multiPrint(int moveNum, int moveSpd) {

  //ATTEMPT #1
  Serial.print(seqNum); 
  Serial.print("_");
  Serial.print(moveNum);
  Serial.print("_");
  Serial.println(moveSpd);

  //ATTEMPT #2
  Serial.print(seqNum); 
  Serial.print("_");
  Serial.print(moveNum);
  Serial.print("_");
  Serial.println(moveSpd);

  //ATTEMPT #3
  Serial.print(seqNum); 
  Serial.print("_");
  Serial.print(moveNum);
  Serial.print("_");
  Serial.println(moveSpd);

  seqNum = seqNum + 1;

}

Mike G. Comments Dec 28, 2008

Variable seqNum needs to be declared somewhere. Will it work as a global variable? Or do you need to change multPrint from a "void" function to one that returns seqNum as a result?

Code trials

ElmoMasterTEST

(working)

int switchPin = 2;
int previousState = 1;
int nowState = 0;
int stateChange = 0;
int ledPin = 13;  //to double check that code is good
int Step = 1;
int seqNum = 1;


void setup() {

  pinMode(switchPin, INPUT);
  digitalWrite(switchPin, HIGH); //enable internal pull-up resistor
  pinMode(ledPin, OUTPUT);
  Serial.begin(9600);

}


void loop(){

  state();

  if (stateChange == 1 && nowState == 1){

    //Serial.println('H');
    digitalWrite(ledPin,HIGH);
    previousState=nowState;
    state();

  }


  else if (stateChange == 1 && nowState == 0) {   

    switch(Step) {
      case 1:
        multiPrint(22, 15);
        break;
      case 2:
        multiPrint(15, 200);
        break;
      case 3:
        multiPrint(15, 300);
        break;
      case 4:
        multiPrint(60, 50);
        break;
      case 5:
        multiPrint(338, 49);
        break;
      case 6:
        multiPrint(45, 90);
        break;
      case 7:
        multiPrint(100, 100);
        break;
      default:
        multiPrint(0, 0);
        break;
    }

    //push the action number forward     
    Step = Step + 1;
    digitalWrite(ledPin,LOW);
    previousState=nowState;
    state();
  }
}


void state() {

  nowState=digitalRead(switchPin);

  if (nowState != previousState){
    stateChange=1;
  }

  else {
    stateChange=0;
  }

}


void multiPrint(int moveNum, int moveSpd) {

  //ATTEMPT #1
  Serial.print(seqNum); 
  Serial.print("_");
  Serial.print(moveNum);
  Serial.print("_");
  Serial.println(moveSpd);

  //ATTEMPT #2
  Serial.print(seqNum); 
  Serial.print("_");
  Serial.print(moveNum);
  Serial.print("_");
  Serial.println(moveSpd);

  //ATTEMPT #3
  Serial.print(seqNum); 
  Serial.print("_");
  Serial.print(moveNum);
  Serial.print("_");
  Serial.println(moveSpd);

  seqNum = seqNum + 1;

}

ElmoSlaveTEST

(working, but cannot interact with ElmoMasterTEST YET)

int val = 0;

//Motor Output Pins:

int armPwmPin = 10;      // L293 pin 1
int armMtrPosPin = 2;    // L293 pin 2
int armMtrNegPin = 3;    // L293 pin 7
int legPwmPin = 11;      // L293 pin 9
int legMtrPosPin = 4;    // L293 pin 15
int legMtrNegPin = 5;    // L293 pin 10


//Connections to drive motors:

// L293 pin 3 - Arm Motor + (green wire)
// L293 pin 6 - Arm Motor - (white wire)
// L293 pin 14 - Leg Motor + (yellow wire)
// L293 pin 11 - Leg Motor - (brown wire)


//Motor Feedback Pins:

int armSensorBrown = 6;    //red wire -20 180+
int armSensorRed = 7;      //white wire 0 90~180
int armSensorYellow = 8;   //yellow wire 90 90
int armSensorGreen = 9;    //grey wire 150 0
int legSensorBlue = 14;    //blue wire 135
int legSensorGreen = 15;   //green wire 90
int legSensorYellow = 16;  //yellow wire 30
int legSensorOrange = 17;  //orange wire 0
int legSensorRed = 18;     //red wire -30


//Gyroscope Feedback Pins:

int gyroGreen = 13;    // supine
int gyroYellow = 19;   // upright
int gyroRed = 12;      // prone
//brown is ground, and orange is not connected


//Variables:

int forward=1;
int reverse=0;
int stepNumber=0; //keep track of which movement to do next
int currentStep;


void setup() {


  Serial.begin(9600);


  //Motor Output Pins:

  pinMode(armPwmPin, OUTPUT);        //Set pin 3 as an output
  pinMode(armMtrPosPin, OUTPUT);     //Set pin 2 as an output
  pinMode(armMtrNegPin, OUTPUT);     //Set pin 4 as an output
  digitalWrite(armPwmPin, LOW);      //Disable motor on startup
  digitalWrite(armMtrPosPin, HIGH);  //Prepare for clockwise motor rotation
  digitalWrite(armMtrNegPin, LOW);   //Prepare for clockwise motor rotation

  pinMode(legPwmPin, OUTPUT);        //Set pin 3 as an output
  pinMode(legMtrPosPin, OUTPUT);     //Set pin 2 as an output
  pinMode(legMtrNegPin, OUTPUT);     //Set pin 4 as an output
  digitalWrite(legPwmPin, LOW);      //Disable motor on startup
  digitalWrite(legMtrPosPin, HIGH);  //Prepare for clockwise motor rotation
  digitalWrite(legMtrNegPin, LOW);   //Prepare for clockwise motor rotation


  //Motor Feedback Pins: for arm

  pinMode(armSensorRed, INPUT);      //Set pin 7 as an input
  digitalWrite(armSensorRed, HIGH);  //Enable internal pull-up resistor

  pinMode(armSensorBrown, INPUT);      //Set pin 8 as an input
  digitalWrite(armSensorBrown, HIGH);  //Enable internal pull-up resistor

  pinMode(armSensorYellow, INPUT);      //Set pin 9 as an input
  digitalWrite(armSensorYellow, HIGH);  //Enable internal pull-up resistor

  pinMode(armSensorGreen, INPUT);      //Set pin 10 as an input
  digitalWrite(armSensorGreen, HIGH);  //Enable internal pull-up resistor


  //Motor Feedback Pins: for legs

  pinMode(legSensorBlue, INPUT);      //Set pin 7 as an input
  digitalWrite(legSensorBlue, HIGH);  //Enable internal pull-up resistor

  pinMode(legSensorGreen, INPUT);      //Set pin 8 as an input
  digitalWrite(legSensorGreen, HIGH);  //Enable internal pull-up resistor

  pinMode(legSensorYellow, INPUT);      //Set pin 9 as an input
  digitalWrite(legSensorYellow, HIGH);  //Enable internal pull-up resistor

  pinMode(legSensorOrange, INPUT);      //Set pin 10 as an input
  digitalWrite(legSensorOrange, HIGH);  //Enable internal pull-up resistor

  pinMode(legSensorRed, INPUT);      //Set pin 10 as an input
  digitalWrite(legSensorRed, HIGH);  //Enable internal pull-up resistor

//  Serial.print("Step: ");
//  Serial.println(stepNumber);

}


void loop() {

  //Wait for a move number+speed message to be sent.
  //Check the sequence number of the message to weed out
  //repeated messages.
  //Execute desired move at desired speed.


  currentStep = stepNumber;

  while (currentStep == stepNumber) {
    stepNumber=waitForButton(stepNumber);
  }

//  Serial.print("...doing stepNumber ");
//  Serial.println(stepNumber);

  doNextStep(stepNumber);

//    moveArm(armSensorGreen, forward, 100);
}


int waitForButton(int whichStep) {

  if (Serial.available()) {
    val = Serial.read();
    //val = digitalRead(switchPin);
  //  while(val == 'L'){
    //   val = Serial.read();


  //  val = digitalRead(switchPin);
    if(val == 'L') {
  //    return whichStep;

    whichStep = whichStep+1;
//    Serial.print("Step: ");
//    Serial.println(whichStep);
      }

  //  return whichStep;
  }

  return whichStep;

}


void doNextStep(int whichStep) {

  switch(whichStep) {
  case 1:
    moveArm(armSensorRed, reverse, 150);
    break;
  case 2:
    moveArm(armSensorGreen, forward, 150);
    break;
  case 3:
    moveArm(armSensorRed, reverse, 150);
    break;
  case 4:
    moveArm(armSensorGreen, forward, 150);
    break;
  case 5:
    moveArm(armSensorYellow, reverse, 150);      
    break;
  case 6:
    moveLeg(legSensorGreen,forward, 200);
    break;
  case 7:
    stepNumber = 0;
    moveLeg(legSensorOrange, reverse, 200);
    break;

  }
}


void blink(int whatPin, int howManyTimes, int milliSecs) {

  int i=0;

  for(i=0;i<howManyTimes;i++) {
    digitalWrite(whatPin, HIGH);
    delay(milliSecs/2);
    digitalWrite(whatPin, LOW);
    delay(milliSecs/2);

  }
}


void moveArm(int armSensorColor, int whichWay, int howFast) {


  //MOVE MOTOR IN DIRECTION OF "whichWay" (EITHER FORWARD OR REVERSE)

  int positivePin = whichWay;
  int negativePin = ~whichWay & 1;
  digitalWrite(armMtrPosPin, positivePin); 
  digitalWrite(armMtrNegPin, negativePin);
  analogWrite(armPwmPin,howFast); // Turn motor on at speed "howFast"


  //WATCH FOR WHEN THE MOTOR REACHES THE NEXT TARGET SENSOR

  int sensor = 1;

  while(sensor==HIGH) {
    sensor = digitalRead(armSensorColor);
  }


  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:

  analogWrite(armPwmPin,0);

}


void moveLeg(int legSensorColor, int whichWay, int howFast) {


  //MOVE MOTOR IN DIRECTION OF "whichWay" (EITHER FORWARD OR REVERSE)
  int positivePin = whichWay;
  int negativePin = ~whichWay & 1;
  digitalWrite(legMtrPosPin, positivePin); 
  digitalWrite(legMtrNegPin, negativePin);
  analogWrite(legPwmPin,howFast); // Turn motor on at speed "howFast"


  //WATCH FOR WHEN THE MOTOR REACHES THE NEXT TARGET SENSOR

  int sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(legSensorColor);
  }


  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:

  analogWrite(legPwmPin,0);

}

ElmoMasterTESTarray

(attempt to send over an array of command values)

This code works, but it sometimes sends crap. Hmm. -Mike S.

//Movement value defines

#define ARM 0
#define LEG 1
#define GYR 2
#define SPD 3

#define armBr 'B'
#define armRd 'R'
#define armYw 'Y'
#define armGn 'G'

#define legBl 'B'
#define legGr 'G'
#define legYw 'Y'
#define legOr 'O'
#define legRd 'R'

#define gyrGr 'G' 
#define gyrYw 'Y'
#define gyrRd 'R'

#define FAST  'A'
#define MED   'B'
#define SLOW  'C'

#define HOLD  'H'


//Variable definitions

int switchPin = 2;
int previousState = 1;
int nowState = 0;
int stateChange = 0;
int ledPin = 13;  //to double check that code is good
int Step = 1;
int seqNum = 1;
char movMsg[4] = {NULL};


void setup() {

  pinMode(switchPin, INPUT);
  digitalWrite(switchPin, HIGH); //enable internal pull-up resistor
  pinMode(ledPin, OUTPUT);
  Serial.begin(9600);

}


void loop(){


  state();

  if (stateChange == 1 && nowState == 1) {

    //Serial.println('H');
    digitalWrite(ledPin,HIGH);
    previousState=nowState;
    state();

  }


  else if (stateChange == 1 && nowState == 0) {       

    switch(Step) {
      case 1:
        movMsg[ARM] = armBr;
        movMsg[LEG] = legBl;
        movMsg[GYR] = gyrGr;
        movMsg[SPD] = FAST;
        multiPrint(movMsg);
        break;
      case 2:
        movMsg[ARM] = armBr;
        movMsg[LEG] = legBl;
        movMsg[GYR] = gyrGr;
        movMsg[SPD] = FAST;
        multiPrint(movMsg);
        break;
      case 3:
        movMsg[ARM] = armBr;
        movMsg[LEG] = legBl;
        movMsg[GYR] = gyrGr;
        movMsg[SPD] = FAST;
        multiPrint(movMsg);
        break;
      case 4:
        movMsg[ARM] = armBr;
        movMsg[LEG] = legBl;
        movMsg[GYR] = gyrGr;
        movMsg[SPD] = FAST;
        multiPrint(movMsg);
        break;
      case 5:
        movMsg[ARM] = armBr;
        movMsg[LEG] = legBl;
        movMsg[GYR] = gyrGr;
        movMsg[SPD] = FAST;
        multiPrint(movMsg);
        break;
      case 6:
        movMsg[ARM] = armBr;
        movMsg[LEG] = legBl;
        movMsg[GYR] = gyrGr;
        movMsg[SPD] = FAST;
        multiPrint(movMsg);
        break;
      case 7:
        movMsg[ARM] = armBr;
        movMsg[LEG] = legBl;
        movMsg[GYR] = gyrGr;
        movMsg[SPD] = FAST;
        multiPrint(movMsg);
        break;
      default:
        movMsg[ARM] = armBr;
        movMsg[LEG] = legBl;
        movMsg[GYR] = gyrGr;
        movMsg[SPD] = FAST;
        multiPrint(movMsg);
        break;
    }

    delay(1000); // *** TESTING ***

    //push the action number forward     
    Step = Step + 1;

    digitalWrite(ledPin,LOW);
    previousState=nowState;
    state();

  }

}


void state() {

  nowState=digitalRead(switchPin);

  if (nowState != previousState){
    stateChange=1;
  }

  else {
    stateChange=0;
  }

}


void multiPrint(char moveMessage[]) {

  //ATTEMPT #1
  Serial.print(seqNum); 
  Serial.print(" ");
  Serial.println(moveMessage);
  delay(10);

  //ATTEMPT #2
  Serial.print(seqNum); 
  Serial.print(" ");
  Serial.println(moveMessage);
  delay(10);

  //ATTEMPT #3
  Serial.print(seqNum); 
  Serial.print(" ");
  Serial.println(moveMessage);
  delay(10);

  //Clear the command array
  moveMessage[ARM] = NULL;
  moveMessage[LEG] = NULL;
  moveMessage[GYR] = NULL;
  moveMessage[SPD] = NULL;

  seqNum = seqNum + 1;

}

This is the start for the Slave's packet parsing code

int ledPin=13;

int waitForButton(int whichStep) {
  Serial.flush();
  char packet[64];
  int i;
  int size=0;
  int test;
  if (Serial.available() > 0) { //If anything is available on the Serial port...
//    if (Serial.read() == 0x11) // "Channel" should be 0x11
    test = Serial.read();
    Serial.println(test,HEX);
    return whichStep;
    {
      if (Serial.read() == 0x22) // "ID" should be 0x22
      {
        if (Serial.read() == 0x01) // if it's a move, it should be 0x01
        {
          size = Serial.read(); //this byte is the size of the data packet
          for (i=0;i<=size-1;i++) {
            packet[i] = Serial.read();
          }
          if (Serial.read() == 0x0A) // LF
          {
            if (Serial.read() == 0x0D) // CR
            {
              if (packet[0] == 'L')
              {
                whichStep = whichStep+1;
                sendAck();
                return whichStep;
              }       
            }
          }
        }
      }
    }
  }
  else {
    return whichStep;
  }
}

This is the code modified by Alex for the Master's packet parsing:


//soyoung Dec 1st 2008
//Xbee sending switch
int switchPin = 2;
int previousState=0;
int nowState=0;
int stateChange=0;
int ledPin=13; //to double check that code is good
char packet[64];


void setup() {
  pinMode(switchPin, INPUT);
  digitalWrite(switchPin, HIGH); //enable internal pull-up resistor
  pinMode(ledPin, OUTPUT);
  Serial.begin(9600);


}
void loop(){
  state();
  if (stateChange == 1 && nowState == 1){
    //Serial.println('H');

    digitalWrite(ledPin,HIGH);
    previousState=nowState;
    state();
  }
  else if (stateChange == 1 && nowState == 0){
    Serial.println(packet);
        transmitCommand(0x22, 'L');
    digitalWrite(ledPin,LOW);
    previousState=nowState;
    state();
  }
}
void state(){
  nowState=digitalRead(switchPin);
  if (nowState != previousState){
    stateChange=1;
  }
  else {
    stateChange=0;
  }
}

void transmitCommand (char elmoID, char command)
{
  char commandOut[1];
  commandOut[0] = command;

    dataTransmit(0x11, elmoID, 0x01, 0x01, commandOut);
    //  while(!checkAck(0x11, elmoID))
 // {

 // }
  return;
}

void dataTransmit(char channel, char elmoID, char packetType, char dataSize, char dataIn[58]){
  //Write values to "packet"
  Serial.println(channel, BYTE);
  Serial.println(elmoID, BYTE);
  Serial.println(packetType, BYTE);
  Serial.println(dataSize, BYTE);
  for (char i = 4; i < dataSize + 4; i++)
  {
    Serial.println(dataIn[i-4], BYTE);
  }
  Serial.println(0x0A, BYTE);
  Serial.println(0x0D, BYTE);
  return;
}



char checkAck(char channel, char elmoID)
{
  if (Serial.available() > 0) {
    if(Serial.read() == channel)
    {
      if(Serial.read() == elmoID)
      {
        if(Serial.read() == 0x02) //packet type
        {
          if(Serial.read() == 0x01) //data size
          {
            if(Serial.read() == 0x06) //ACK
            {
              if(Serial.read() == 0x0A) //LF
              {
                if(Serial.read() == 0x0D) //CR
                {
                  return 1;
                }
              }
            }
          }
        }
      }
    } 
    return 0; 


  }
  else
  {
    return 0;
  }

}

Edit - History - Print - Recent Changes - Search
Page last modified on February 15, 2009, at 01:50 PM