The problem I am having is with manual (bluetooth) control. My idea is that when I have bluetooth available, it will only run the bluetooth loops. It sort of does it, but it will run for a second, stop, run for a second, stop...I am trying to get continous control while my bluetooth is connected. I have connected a pin to my HC05 bluetooth module so when the led is on (bluetooth connected) it feeds an analog signal to A01 and in the code I am using "if (analogRead(inputPin)>0)" as a switch between autonomous mode and manual bluetooth mode. The "Stop ()" loop that I call right after bluetooth enable is to stop the robot from moving after taking manual control...it does not appear to be what is causing the stuttering in manual mode. Other than the manual mode stutter, the code works fine.
Thanks for taking the time to help. I really apprecitate it, especially since I am not a programmer.
Code below---------------------
#define M1_DIR 8 // Arduino pin that controls the motor address (DIGITAL)
#define M1_PWM 9 // Arduino pin that controls the speed (PWM)
#define M2_DIR 10 // Arduino pin that controls the motor address (DIGITAL)
#define M2_PWM 11 // Arduino pin that controls the speed (PWM)
#define MODE 15 //DRV8835 mode enable through pin 15 (SCK)
int left_trig = 7;
int left_echo = 6;
int right_trig = 3;
int right_echo = 2;
long front_time, front_dis, right_time, right_dis, left_time, left_dis;
const int forwardspeed=255;
const int reversespeed=-255;
const int halt=0;
int inputPin = A1;
int motorLspeed; // controls the speed of the left hand motor: 255= full forward, 0=halt
int motorRspeed; // controls the speed of the right hand motor: -255= full reverse
int joyX=0; //the position of the bluebots joystic on the x axis
int joyY=0; // the position of the bluebots joytick on the y axis
int slide2=0; // the position of one slider on the bluebots interface
int slide1=0; // the position of the other slider on the bluebots interface
unsigned long last_time=millis(); // this will be used to check for the time delay between
// receiving signals. If it is too long, the robot will stop
int checksum=0; // the checksum is used to ensure that the bluebots data package is correct
int double_check=0; // the checksum is compared to the "double_check" variable
void setup() {
pinMode(left_trig, OUTPUT);
pinMode(left_echo, INPUT);
pinMode(right_trig, OUTPUT);
pinMode(right_echo, INPUT);
pinMode ( M1_DIR , OUTPUT ) ;
pinMode ( M1_PWM , OUTPUT ) ;
pinMode ( M2_DIR , OUTPUT ) ;
pinMode ( M2_PWM , OUTPUT ) ;
pinMode (MODE, OUTPUT);
digitalWrite (MODE, HIGH);
Serial.begin(9600); //set up the serial port to feed data back to the PC
Serial1.begin(9600);
}
void loop() {
if (analogRead(inputPin)>0)
while (Serial1.available()>0)
{Stop ();
drive(motorLspeed,motorRspeed); // execute the drive function to set the motor speeds
read_bt(); // execute the read_bt function to read the bluetooth signal
}else{
digitalWrite(left_trig, LOW);
delayMicroseconds(2);
digitalWrite(left_trig, HIGH);
delayMicroseconds(5);
digitalWrite(left_trig, LOW);
left_time= pulseIn(left_echo, HIGH);
left_dis= left_time / 29 / 2;
digitalWrite(right_trig, LOW);
delayMicroseconds(2);
digitalWrite(right_trig, HIGH);
delayMicroseconds(5);
digitalWrite(right_trig, LOW);
right_time = pulseIn(right_echo, HIGH);
right_dis = right_time / 29 / 2;
Serial.print(right_dis);
Serial.print(":");
Serial.print(":");
Serial.println(left_dis);
if(right_dis <= 15 && left_dis > 30)
{ left();
}
else if(left_dis <= 15 && right_dis > 30)
{right();
}
else if (right_dis <= 19 && left_dis <=19)
{back();
}else {
forward();
}
delay(100);
}
}
void read_bt(){
if (Serial1.available()>0) { //this checks to see if a byte of serial data has been received
double_check=0; // if it has, then reset the double_check variable
if (Serial1.read()==125) { //125 indicates a joystick packet... only do this if it is a joystick package
joyX=Serial1.read(); // type mismatch issues occur
if (joyX>128) joyX=joyX-256; //so make sure we get the negatives correct
joyY=Serial1.read();
if (joyY>128) joyY=joyY-256;
slide2=Serial1.read();
slide1=Serial1.read();
checksum=Serial1.read(); //see Bluebots help file for checksum
double_check=(125+joyX+joyY+slide1+slide2)%256;
last_time=millis();
}
}
// Timeout routine to stop motors if signal is lost for 500ms
// also stops the motors if the checksum indicates lost data
// This section would be better if I used 2x oversampling and
// only killed the motors if two checksums in sequence were flawed
// This section can be commented out, but it does play a safety role
// in the event that Bluetooth communication is lost
if ((millis()-last_time)>500||(checksum!=double_check)){
joyX=0;
joyY=0;
slide2=62;
slide1=62;
if (millis()-last_time>500)Serial.println("Time Out");
if (checksum!=double_check)Serial.println("Checksum Error");
}
// Map and send motor outputs
motorLspeed=joyY-joyX;
motorLspeed=motorLspeed*2;
motorRspeed=joyY+joyX;
motorRspeed=motorRspeed*2;
// Debugging print routine
Serial.print("X");
Serial.print(joyX);
Serial.print(" Y");
Serial.print(joyY);
//Serial.print(" S1 ");
//Serial.print(slide1);
//Serial.print(" S2 ");
//Serial.print(slide2);
//Serial.print(" Chk ");
//Serial.print(checksum);
//Serial.print(" DblChk ");
//Serial.print((double_check));
// Serial.print("IR:");
// Serial.print(analogRead(rangefinder));
// Serial.print(" EdgL:");
// Serial.print(analogRead(edge_left));
// Serial.print(" EdgR:");
// Serial.print(analogRead(edge_right));
Serial.print(" MotL ");
Serial.print(motorLspeed);
Serial.print(" MotR ");
Serial.println(motorRspeed);
delay(100);
}
void drive(int left_speed,int right_speed){
if (left_speed > forwardspeed) left_speed=forwardspeed;
if (left_speed < reversespeed) left_speed=reversespeed;
if (right_speed > forwardspeed) right_speed=forwardspeed;
if (right_speed < reversespeed) right_speed=reversespeed;
if (right_speed > 0){
digitalWrite(M2_DIR , HIGH);
analogWrite(M2_PWM,right_speed);
}
if (right_speed < 0){
digitalWrite(M2_DIR,LOW);
analogWrite(M2_PWM,abs(right_speed));
}
if (right_speed == 0){
digitalWrite(M2_DIR,LOW);
digitalWrite(M2_PWM,0);
}
if (left_speed > 0){
digitalWrite(M1_DIR,HIGH);
analogWrite(M1_PWM,left_speed);
}
if (left_speed < 0){
digitalWrite(M1_DIR,LOW);
analogWrite(M1_PWM,abs(left_speed));
}
if (left_speed == 0){
digitalWrite(M1_DIR,LOW);
digitalWrite(M1_PWM,0);
}
}
void right() {
// turn right
digitalWrite ( M1_DIR , HIGH ) ; // Motor one goes back
analogWrite ( M1_PWM , 50 ) ;
digitalWrite ( M2_DIR , LOW ) ; // Engine advances
analogWrite ( M2_PWM , 80 ) ;
}
void Stop () {
digitalWrite ( M1_DIR , LOW ) ;
analogWrite ( M1_PWM , 0 ) ;
digitalWrite ( M2_DIR , LOW ) ;
analogWrite ( M2_PWM , 0 ) ; // Stop
}
void left() {
// turn left
digitalWrite ( M1_DIR , HIGH ) ; // Engine one advances
analogWrite ( M1_PWM , 80 ) ;
digitalWrite ( M2_DIR , LOW ) ; // Engine dos back
analogWrite ( M2_PWM , 50 ) ;
}
void forward() {
//go forward
//add 10% to motor 2 to even speeds.
digitalWrite ( M1_DIR , HIGH ) ;
analogWrite ( M1_PWM , 89 ) ;
digitalWrite ( M2_DIR , HIGH ) ;
analogWrite ( M2_PWM , 80 ) ;
}
void back(){
// go backwards
digitalWrite ( M1_DIR , LOW ) ;
analogWrite ( M1_PWM , 100 ) ;
digitalWrite ( M2_DIR , LOW ) ;
analogWrite ( M2_PWM , 100 ) ;
delay (1400);
}