|
ENTER NEW DAILY DATA HERE:
|
Site /
December24-MandNsCodeNotebookThis 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, 2008Variable 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 trialsElmoMasterTEST(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;
}
}
|