Ok, so the trick with having roborealm communicate to the arduino is by sending a variable [ArduinoCommand] from the vbscript module to the serial module where the arduino is... Best way to send multiple variables to the arduino at one time - is to send a string like 111 180 90 1, where the arduino reads serial data and creates a string until it hits the comma (,) then it turns that string into an integer the first set of three being for the case statement that determines what the arduino does and what string of integers it returns.
Pretty simple right. well here you go.
-spacehermit
//this one is more advanced then what you'll be needing..
//it reads in a string of integers.
//-spacehermit@gmail.com
#include <avr/io.h>
#include <avr/interrupt.h>
#include <WString.h> //provides easy string handling
#include <stdio.h>
#include <stdlib.h>
#include <Servo.h> //provides the standard servo functions
//int OCR2APin = 11; //PWM Changeable Pin
//int OCR2BPin = 10;
//int analogpin = 0;
volatile int pwm;
//volatile int duty = 127 ;
//volatile int duty2 = 127 ;
volatile int duty = 0 ;
volatile int duty2 = 0 ;
String writeString = String(45); //stores the entire string up to a comma
String readString = String(45); //stores the entire string up to a comma
String parce0 = String(4); //first part of the string up to a space
String parce1 = String(4); //second part of the string up to a space
String parce2 = String(4); //third part of the string up to a space
String parce3 = String(4); //fourth part of the string up to a space
String parce4 = String(4); //fifth part of the string up to a space
String parce5 = String(4); //sixth part of the string up to a space
String parce6 = String(4); //seventh part of the string up to a space
String parce7 = String(4); //eighth part of the string up to a space
String parce8 = String(4); //nineth part of the string up to a space
String parce9 = String(4); //tenth part of the string up to a space
int pos = 0; //stores the length of the string
int spc1 = 0; //position of first space in string
int spc2 = 0; //position of second space in string
int spc3 = 0; //position of third space in string
int spc4 = 0; //position of fourth space in string
int spc5 = 0; //position of fifth space in string
int spc6 = 0; //position of sixth space in string
int spc7 = 0; //position of seventh space in string
int spc8 = 0; //position of eighth space in string
int spc9 = 0; //position of nineth space in string
//Note that Character Arrays always have a length + 1 for the "\0" at the end
//So these Character Arrays will only hold a 3 digit number
char CAparce0[4]; //character array of the first part of the string
char CAparce1[4]; //character array of the second part of the string
char CAparce2[4]; //character array of the third part of the string
char CAparce3[4]; //character array of the fourth part of the string
char CAparce4[4]; //character array of the fifth part of the string
char CAparce5[4]; //character array of the sixth part of the string
char CAparce6[4]; //character array of the seventh part of the string
char CAparce7[4]; //character array of the eighth part of the string
char CAparce8[4]; //character array of the nineth part of the string
char CAparce9[4]; //character array of the tenth part of the string
//need to setup a variable to tell whether this board is the front or rear control board
// create servo object to control a servo
Servo FLPS; //Front Left Ping Servo
Servo FRPS; //Front Right Ping Servo
//Servo RLPS; //Rear Left Ping Servo
//Servo RRPA; //Rear Right Ping Servo
Servo FCHS; //Front Camera Horizontal Servo
Servo FCVS; //Front Camera Vertical Servo
//Servo RCHS; //Rear Camera Horizontal Servo
//Servo RCVS; //Rear Camera Vertical Servo
int DrivePWMOn = 0;
//Servo WDHPPV;
//Servo WDVPPV;
//Set Pins For Pings
const int FLPP = 3; //Front Left Ping Pin
const int FRPP = 7; //Front Right Ping Pin
//const int RLPP = 3; //Rear Left Ping Pin
//const int RRPP = 11; //Rear Right Ping Pin
//const int GunRelayPin = 9; //Sets Gun Relay Pin
//const int WDHPP = 9; //Wheelchair Drive Horizontal PWM Pin
//const int WDVPP = 10; //Wheelchair Drive Vertical PWM Pin
int FLPDD = 4; //Front Left Ping Danger Distance
int FRPDD = 4; //Front Right Ping Danger Distance
//int RLPDD = 4; //Rear Left Ping Danger Distance
//int RRPDD = 4; //Rear Right Ping Danger Distance
int WDHPV = 100; //Wheelchair Drive Horizontal PWM Value - Default
int WDVPV = 100; //Wheelchair Drive Vertical PWM Value - Default
int WDHPVC = 100; //Wheelchair Drive Horizontal PWM Value - Center
int WDHPVH = 180; //Wheelchair Drive Horizontal PWM Value - High
int WDHPVL = 5; //Wheelchair Drive Horizontal PWM Value - Low
int WDVPVC = 100; //Wheelchair Drive Vertical PWM Value - Center
int WDVPVH = 180; //Wheelchair Drive Vertical PWM Value - High
int WDVPVL = 5; //Wheelchair Drive Vertical PWM Value - Low
int FLPSV = 90; //Front Left Ping Servo Value - Default
int FRPSV = 90; //Front Right Ping Servo Value - Default
//int RLPSV = 90; //Rear Left Ping Servo Value - Default
//int RRPSV = 90; //Rear Right Ping Servo Value - Default
int FCHSV = 90; //Front Camera Horizontal Servo Value - Default
int FCVSV = 90; //Front Camera Vertical Servo Value - Default
//int RCHSV = 90; //Rear Camera Horizontal Servo Value - Default
//int RCVSV = 90; //Rear Camera Vertical Servo Value - Default
int ArduinoDriveStatus = 1; //Will be passed back and forth to RR clear todrive
int RRDriveStatus = 1; //Will tell the Arduino what kind of drive command is sent
int ObjectDetected = 0; //This value will change based on which pings see what
int FLPObjectDetected = 0;
int FRPObjectDetected = 0;
//int RLPObjectDetected = 0;
//int RRPObjectDetected = 0;
int DriveCommand = 0; //for if statement to execute driving action
long FLPInches = 0; //global variable that stores distance to object
long FRPInches = 0; //global variable that stores distance to object
//long RLPInches = 0; //global variable that stores distance to object
//long RRPInches = 0; //global variable that stores distance to object
int Relay = 4;
void setup()
{
Serial.begin(115200);
FLPS.attach(2); //Front Left Ping Servo Pin
FRPS.attach(8); //Front Right Ping Servo Pin
// RLPS.attach(2); //Rear Left Ping Servo Pin
// RRPS.attach(8); //Rear Right Ping Servo Pin
FCHS.attach(12); //Front Camera Horivontal Servo Pin
FCVS.attach(13); //Front Camera Vertical Servo Pin
// RCHS.attach(12); //Rear Camera Horivontal Servo Pin
// RCVS.attach(13); //Rear Camera Vertical Servo Pin
pinMode(3, OUTPUT);
pinMode(11, OUTPUT);
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20);
TCCR2B = _BV(CS22);
// OCR2A = 180;
// OCR2B = 50;
// pinMode ( OCR2APin, OUTPUT);
// pinMode ( OCR2BPin, OUTPUT);
// pinMode (10, OUTPUT); //PWM Changeable Pin
// pinMode (9, OUTPUT); //PWM Changeable Pin
// set dutycycle == 0
OCR2A = duty ;
OCR2B = duty2 ;
// TCCR2B &= 0xF0 ; // preserve 4 msb bits
//TCCR2B |= _BV(CS21) | _BV(CS20); // prescale = 8
// TCCR2B |= _BV(CS21) ; //prescale = 8 ==> ~ 8k frequency
// SET MODE 3 ( waveform Generation, fast pwm ) page 117
// TCCR2A |= _BV(WGM21) | _BV(WGM20);
//TCCR2A |= ( 1 << WGM21 | 1 <<WGM20);
// SET COMPARE OUTPUT, FAST PWM MODE
// CLEAR OC2 ON COMPARE MATCH, non-inverting mode
// TCCR2A |= _BV( COM2A1);
// TIMER/COUNTER INTERRUPT MASK REGISTER (TIMSK)
// SET OUTPUT COMPARE MATCH
// bit_on ( TIMSK2, OCIE2A) ;
// TIMSK2 |= _BV( OCIE2A);
// START FAST PWM MODE
//bit_on( SREG, SREG_I);
// SREG |= _BV( SREG_I);
//WDHPPV.attach(9);
//WDVPPV.attach(10);
//pinMode(WDHPP, OUTPUT); // sets the pin as output
//pinMode(WDVPP, OUTPUT); // sets the pin as output
pinMode(Relay, OUTPUT); //Set Pin4 as output
}
void loop()
{
//while loop makes the string readString
//expect a string like "111 5 90 180 180 1 1 4 4 55,"
while (Serial.available()) {
//delay(10);
if (Serial.available() >0) {
char c = Serial.read(); //gets one byte from serial buffer
//if it's a comma then don't add to string and skip ahead
if (c == ',') { goto parce;}
//only happens till there is a comma
readString = readString + c; //puts the char at end of the string
} //end of serial available if statement
} //end of serial available while statement
parce:
if (readString.length() >0) {
pos = readString.length(); //capture string length
spc1 = readString.indexOf(' '); //position of the first space
spc2 = readString.indexOf(' ', spc1 + 1); //position of the second space
spc3 = readString.indexOf(' ', spc2 + 1); //position of the third space
spc4 = readString.indexOf(' ', spc3 + 1); //position of the fourth space
spc5 = readString.indexOf(' ', spc4 + 1); //position of the fifth space
spc6 = readString.indexOf(' ', spc5 + 1); //position of the sixth space
spc7 = readString.indexOf(' ', spc6 + 1); //position of the seventh space
spc8 = readString.indexOf(' ', spc7 + 1); //position of the eighth space
spc9 = readString.indexOf(' ', spc8 + 1); //position of the nineth space
parce0 = readString.substring(0, spc1); //first part of string
parce1 = readString.substring(spc1+1, spc2); //second part of string
parce2 = readString.substring(spc2+1, spc3); //third part of string
parce3 = readString.substring(spc3+1, spc4); //fourth part of string
parce4 = readString.substring(spc4+1, spc5); //fifth part of string
parce5 = readString.substring(spc5+1, spc6); //sixth part of string
parce6 = readString.substring(spc6+1, spc7); //seventh part of string
parce7 = readString.substring(spc7+1, spc8); //eigth part of string
parce8 = readString.substring(spc8+1, spc9); //ninth part of string
parce9 = readString.substring(spc9+1, pos); //tenth part of string
readString=""; //empties the readString for the next command
parce0.toCharArray(CAparce0, 4); //turns first part of string into char array
parce1.toCharArray(CAparce1, 4); //turns second part of string into char array
parce2.toCharArray(CAparce2, 4); //turns third part of string into char array
parce3.toCharArray(CAparce3, 4); //turns fourth part of string into char array
parce4.toCharArray(CAparce4, 4); //turns fifth part of string into char array
parce5.toCharArray(CAparce5, 4); //turns sixth part of string into char array
parce6.toCharArray(CAparce6, 4); //turns seventh part of string into char array
parce7.toCharArray(CAparce7, 4); //turns eighth part of string into char array
parce8.toCharArray(CAparce8, 4); //turns nineth part of string into char array
parce9.toCharArray(CAparce9, 4); //turns tenth part of string into char array
int kommand, value1, value2, value3, value4, value5, value6, value7, value8, value9;
//will be the integer values of parsed string
kommand = atoi(CAparce0); //turns the first char array into an integer
value1 = atoi(CAparce1); //turns the second character array into an integer
value2 = atoi(CAparce2); //turns the third character array into an integer
value3 = atoi(CAparce3); //turns the fourth character array into an integer
value4 = atoi(CAparce4); //turns the fifth character array into an integer
value5 = atoi(CAparce5); //turns the sixth character array into an integer
value6 = atoi(CAparce6); //turns the seventh character array into an integer
value7 = atoi(CAparce7); //turns the eighth character array into an integer
value8 = atoi(CAparce8); //turns the nineth character array into an integer
value9 = atoi(CAparce9); //turns the tenth character array into an integer
parce0 = ""; //clears the parced segment
parce1 = ""; //clears the parced segment
parce2 = ""; //clears the parced segment
parce3 = ""; //clears the parced segment
parce4 = ""; //clears the parced segment
parce5 = ""; //clears the parced segment
parce6 = ""; //clears the parced segment
parce7 = ""; //clears the parced segment
parce8 = ""; //clears the parced segment
parce9 = ""; //clears the parced segment
pos = 0; //clears the string length count
spc1 = 0; //clears the pointer to the first space
spc2 = 0; //clears the pointer to the second space
spc3 = 0; //clears the pointer to the third space
spc4 = 0; //clears the pointer to the fourth space
spc5 = 0; //clears the pointer to the fifth space
spc6 = 0; //clears the pointer to the sixth space
spc7 = 0; //clears the pointer to the seventh space
spc8 = 0; //clears the pointer to the eighth space
spc9 = 0; //clears the pointer to the nineth space
int i; //simple counter variable for this for loop
for (i = 0; i < 4; i = i + 1) {
CAparce0[i] = '0'; //puts zeros in the the character first array
CAparce1[i] = '0'; //puts zeros in the the character second array
CAparce2[i] = '0'; //puts zeros in the the character third array
CAparce3[i] = '0'; //puts zeros in the the character fourth array
CAparce4[i] = '0'; //puts zeros in the the character fifth array
CAparce5[i] = '0'; //puts zeros in the the character sixth array
CAparce6[i] = '0'; //puts zeros in the the character seventh array
CAparce7[i] = '0'; //puts zeros in the the character eighth array
CAparce8[i] = '0'; //puts zeros in the the character nineth array
CAparce9[i] = '0'; //puts zeros in the the character tenth array
} //end for loop
switch (kommand)
{
//Usual String of Variables when driving
//kommand WDHPV WDVPV FLPSV FRSPV FLDD FRDD ArduinoDriveStatus RRDriveStatus ObjectDetected,
/*
ObjectDetected
0 = no objects detected
1 = FLPS
2 = FRPS
3 = FLPS & FRPS
4 = FLPS & FRPS & RLPS
5 = FLPS & FRPS & RLPS & RRPS
6 = FLPS & FRPS & RRPS
7 = FLPS & RLPS
8 = FLPS & RLPS & RRPS
9 = FLPS & RRPS
10 = FRPS & RLPS
11 = FRPS & RLPS & RRPS
12 = FRPS & RRPS
13 = RLPS
14 = RLPS & RRPS
15 = RRPS
*/
case 1: //Normal Drive Command From RoboRealm
WDHPV = value1;
WDVPV = value2;
FLPSV = value3;
FRPSV = value4;
FLPDD = value5;
FRPDD = value6;
ArduinoDriveStatus = value7;
RRDriveStatus = value8;
ObjectDetected = value9;
DriveCommand = 1; //will execute if statements necessary for driving action
kommand = 0; //resets case statement for next time
break;
case 7:
digitalWrite(Relay, HIGH); //Turn on relay
//reset the commands
kommand = 0;
break;
case 9:
digitalWrite(Relay, LOW); //Turn off relay
//reset the commands
kommand = 0;
break;
case 100: //initialization and reset variables
duty = value1;
duty2 = value2;
OCR2A = duty ;
OCR2B = duty2 ;
WDHPV = value1;
WDVPV = value2;
// DrivePWMOn = 1;
if(DrivePWMOn == 1)
{
analogWrite(9, WDVPV);
analogWrite(10, WDHPV);
}
//WDHPPV.write(WDHPV);
//WDVPPV.write(WDVPV);
writeString = "PWM values ";
writeString = writeString + WDHPV;
writeString = writeString + " ";
writeString = writeString + WDVPV;
Serial.print(writeString);
Serial.println();
writeString=""; //empties the writeString for the next response
value1 = 0;
value2 = 0;
value3 = 0;
value4 = 0;
value5 = 0;
value6 = 0;
value7 = 0;
value8 = 0;
value9 = 0;
kommand = 0;
break;
} //end of switch statement
} //end of parce's if statement
//Code that keeps getting exectuted when DriveCommand == 1
if(DriveCommand == 1)
{
if((WDVPV > WDVPVC) & (WDHPV == WDHPVC)) //Robot is told to move forward
{
//move front ping servos into position and start pinging
FLPS.write(FLPSV);
FRPS.write(FRPSV);
//Function to execute forward Ping Sensors
PingBothFront();
//Check to see if any objects within danger distance
if((FLPInches < FLPDD) & (FLPInches != 0))
{
FLPObjectDetected = 1;
}
else
{
FLPObjectDetected = 0;
}
if((FRPInches < FRPDD) & (FRPInches != 0))
{
FRPObjectDetected = 1;
}
else
{
FRPObjectDetected = 0;
}
//change ArduinoDriveStatus to reflect if there is an object detected
if((FRPObjectDetected == 1) || (FRPObjectDetected == 1))
{
ArduinoDriveStatus = 2;
}
else
{
ArduinoDriveStatus = 1;
}
} //end of if (WDVPV > WDPVC) & (WDHPV == WDHPC)
//make more of these and what not
//Set WDHPV and WDVPV only if no objects detected in path
if((ArduinoDriveStatus != 2) || (ArduinoDriveStatus != 6) || (ArduinoDriveStatus !=
7))
{
// analogWrite(WDVPP, WDVPV);
// analogWrite(WDHPP, WDHPV);
}
/*
if(RRDriveStatus == 9
8
7
6
5
4
3
2
1
if(ObjectDetected ==
*/
} //end of if DriveCommand == 1
} //end of void loop
long PingBothFront()
{
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long durationFLP, inchesFLP;
long durationFRP, inchesFRP;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(FLPP, OUTPUT);
pinMode(FRPP, OUTPUT);
digitalWrite(FLPP, LOW);
digitalWrite(FRPP, LOW);
delayMicroseconds(2);
digitalWrite(FLPP, HIGH);
digitalWrite(FRPP, HIGH);
delayMicroseconds(5);
digitalWrite(FLPP, LOW);
digitalWrite(FRPP, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(FLPP, INPUT);
pinMode(FRPP, INPUT);
durationFLP = pulseIn(FLPP, HIGH);
durationFRP = pulseIn(FRPP, HIGH);
// convert the time into a distance
inchesFLP = microsecondsToInches(durationFLP);
inchesFRP = microsecondsToInches(durationFRP);
FLPInches = inchesFLP;
FRPInches = inchesFRP;
} //end of PingFront function
//Distance Calculation Functions
long microsecondsToInches(long microseconds)
{
// According to Parallax's datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
// See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}
//ISR( TIMER2_COMPA_vect) {
//int analog = analogRead( analogpin)/4;
// OCR2A = duty;
// OCR2B = duty2;
//}
|
|
This Thread
