[SOLVED] Bluetooth Making Servos Move Erratically

Using Arduino Networking, Protocols, and Devices
1

Hi! So I am creating a robot using Bluetooth and processing, and I am having an issue. The Bluetooth receives the signal properly and moves the servos/motors accordingly, but aside from that all of the servos will just start spazzing. There's no clear reason for this, as when I tested it through the regular serial connection it was just fine. Just some background, the Arduino is supposed to take a code and deconstruct the string in order to control a motor/servo. For example, ""; "<" and ">" signify the start and end of the message. The "B" tells the program which motor/servo to control, and the "200" signifies the speed/mapping of the servo/motor. The reason this is necessary is that I have a Processing code running in the background taking inputs from my controller and sending mapping data over the Serial port. The "Claw" servo seems to work at the beginning where I map it so that it does not stall at the close, but completely stops working after that. Does anyone have any idea why that might be? Any help would be appreciated.

Arduino 1st Tab:

#include <Wire.h>
#include <Servo.h>
#include <Adafruit_MotorShield.h>
#include <SoftwareSerial.h>

SoftwareSerial hc06(2,3);

//set up motorhsield
Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x60);

//assign motors
Adafruit_DCMotor *RailMotor = AFMS.getMotor(1);
Adafruit_DCMotor *Swivel = AFMS.getMotor(2);
Adafruit_DCMotor *Screw = AFMS.getMotor(3);

//call the servo library
Servo DrillArm, ClawArm, Claw, Elbow;

const byte number_of_chars = 32;
char receivedChars[number_of_chars];
boolean new_data = false;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  hc06.begin(9600);
  // create with the default frequency 1.6KHz
  AFMS.begin();
  //assign servos
  Elbow.attach(12);
  DrillArm.attach(11);
  ClawArm.attach(10);
  Claw.attach(9);
  Claw.write(75);
}


void loop() {
  receivePacket();
  if (new_data == true) {
    parseData();
    executeParsedData();
    new_data = false;
    char buf[4] = {receivedChars[1], receivedChars[2], receivedChars[3], receivedChars[4]};
    int mapp = atoi(buf);

    if (receivedChars[0] == 'A'){ //claw arm control
      ClawArm.write(mapp);
    }

    else if(receivedChars[0] == 'B'){ //elbow control
      Elbow.write(mapp);
    }
    
    else if(receivedChars[0] == 'C'){ //claw open or close
      Claw.write(mapp);
    }

    else if(receivedChars[0] == 'D'){ //drill arm control
      DrillArm.write(mapp);
    }

    else if(receivedChars[0] == 'E'){ //rail control
      if (mapp > 20){
        RailMotor->setSpeed(mapp);
        RailMotor->run(FORWARD);
      }
      else if (mapp < -20){
        RailMotor->setSpeed(abs(mapp));
        RailMotor->run(BACKWARD);
      }
      else {
        RailMotor->run(RELEASE);
      }
    }

    else if(receivedChars[0] == 'F'){ //swivel control
      if (mapp > 15){
        Swivel->setSpeed(mapp);
        Swivel->run(FORWARD);
      }
      else if (mapp < -15){
        Swivel->setSpeed(abs(mapp));
        Swivel->run(BACKWARD);
      }
      else {
        Swivel->run(RELEASE);
      }
    }
    
    else if(receivedChars[0] == 'G'){ //screw control
      if (mapp > 15){
        Screw->setSpeed(mapp);
        Screw->run(FORWARD);
      }
      else if (mapp < -15){
        Screw->setSpeed(abs(mapp));
        Screw->run(BACKWARD);
      }
      else {
        Screw->run(RELEASE);
      }
    }
   
  }
}

Arduino 2nd Tab:

#include <SoftwareSerial.h>
#include <Wire.h>

int red_led = 3;
int green_led = 6;
int blue_led = 5;
int int_1 = 0;
int int_2 = 0;
int int_3 = 0;

void receivePacket() {
  digitalWrite(LED_BUILTIN, LOW);
  static boolean receiving = false;
  static byte index = 0;
  char start_mark = '<';
  char end_mark = '>';
  char rc;
  while (hc06.available() > 0 && new_data == false) {
    rc = hc06.read();
    if (receiving == true) {
      if (rc != end_mark) {
        receivedChars[index] = rc;
        index++;
        if (index >= number_of_chars) {
          index = number_of_chars-1;
        }
      } else {
        receivedChars[index] = '\0';
        receiving = false;
        index = 0;
        new_data = true;
      }
    } else if (rc == start_mark) {
      receiving = true;
    }
  }
}

void parseData() {
  char * split;
  split = strtok(receivedChars, ",");
  int_1 = atoi(split);
  split = strtok(NULL, ",");
  int_2 = atoi(split);
  split = strtok(NULL, ",");
  int_3 = atoi(split);
}

void executeParsedData() {
  if (int_3 == 0) {
    analogWrite(red_led, 0);
    analogWrite(green_led, 0);
    analogWrite(blue_led, 0);
  } else if (int_3 == 1) {
    analogWrite(red_led, 255);
    analogWrite(green_led, 255);
    analogWrite(blue_led, 255);
  } else if (int_3 == 2) {
    if (int_2 == 0) analogWrite(red_led, int_1);
    if (int_2 == 1) analogWrite(green_led, int_1);
    if (int_2 == 2) analogWrite(blue_led, int_1);
  }
}

Processing:

import processing.serial.*;
import net.java.games.input.*;
import org.gamecontrolplus.*;
import org.gamecontrolplus.gui.*;

Serial port;
ControlDevice cont;
ControlIO control;
float swivel, screw, rail, selectcontrol;
boolean shoulderselect, elbowselect, clawarmselect, clawgrab, clawrelease;
String portmessage, string;
PImage splash;

void setup(){
  size(700, 400);
  control = ControlIO.getInstance(this);
  cont = control.getMatchedDevice("STAR_2021_Artificial_Astronauts_controls");
  
  if (cont == null) {
    println("No suitable device configured, exiting program"); 
    System.exit(-1);
  }
  
  splash = loadImage("Artificial Astronaut Control Scheme");
  println((Object[])Serial.list());
  port = new Serial(this, "COM5", 9600);
}

public void getUserInput() {
  screw = map(cont.getSlider("Screw").getValue(), -1, 1, 0, 180);
  swivel = map(cont.getSlider("SwivelJoint").getValue(), -1, 1, 0, 180);
  rail = map(cont.getSlider("Rail").getValue(), -1, 1, 0, 180);
  selectcontrol = map(cont.getSlider("SelectControl").getValue(), -1, 1, 0, 360);
  shoulderselect = cont.getButton("ShoulderSelect").pressed();
  elbowselect = cont.getButton("ElbowSelect").pressed();
  clawarmselect = cont.getButton("ClawArmSelect").pressed();
  clawgrab = cont.getButton("ClawGrab").pressed();
  clawrelease = cont.getButton("ClawRelease").pressed();
}


            
void draw() {
  getUserInput();
  background(selectcontrol,400,400);
  //send a list that consists of the joint's name and, position(and direction) to the serial for the arduino
  if(clawarmselect){ //control claw arm with right stick
    println("Selecting the claw arm to control, use the right stick to control");
    portmessage = "<A"+str((int)selectcontrol/2)+">";
    port.write(portmessage);
    clawarmselect = false;
  }
  
  if(elbowselect){ //control elbow with button and control right stick
    println("Selecting the elbow to control, use the right stick to control: ");
    portmessage = "<B"+str(((int)selectcontrol/2))+">";
    port.write(portmessage);
    print(portmessage);
    elbowselect = false;
  }
  
  if(clawgrab){ //close claw
    println("Claw Closing");
    port.write("<C75>");
    clawgrab = false;
  }
  
  if(clawrelease){ //open claw
    println("Claw Opening");
    port.write("<C0>");
    clawrelease = false;
  }
  
  if(shoulderselect){ //control elbow with button and control right stick
    println("Selecting the shoulder to control, use the right stick to control: ");
    portmessage = "<D"+str((int)selectcontrol)+">";
    port.write(portmessage);
    shoulderselect = false;
  }
  
  if(rail > 105 | rail < 80){ //control elbow with button and control right stick
    println("Selecting the rail to control, use the left stick to control: ");
    portmessage = "<E"+str((int)rail)+">";
    print(portmessage);
    port.write(portmessage);
  }
  
  if(swivel > 105 | swivel < 80){ //control swivel with button and control right stick
    println("Selecting the swivel to control, use the left stick to control: ");
    portmessage = "<F"+str(((int)swivel))+">";
    print(portmessage);
    port.write(portmessage);
  }
  
  if(screw > 105 | screw < 75){ //control screw with trigger buttons, right trigger goes to -1, left goes to 1
    println("Screw: ");
    portmessage = "<G"+str((int)screw-90)+">";
    port.write(portmessage);
    print(portmessage);
  }
}
2

Any time you code a "while", you are setting yourself for conditions as you describe.
Paul

3

I took the one "while" out of my code and it still presents the same unwanted jittery behaviour.

4

Look for other code that BLOCKS continuous execution of your program. "while" is just one possibility.
Paul

5

The thing is it still works outside of the servos being jittery, but I see no reason why it would be doing that since it wasn't doing that when I had it connected via USB.

6

The hc06 will use more power than a usb connection. How is everything being powered?

Have you tried with the hc06 connected to Serial instead of using software serial?

7

A 9V battery is connected to the Arduino whenever I am testing it.

8

If it is one of the 9v pp3 smoke alarm batteries, you should try something else. They are very poor power sources.

9

OK turns out it wasn't the batteries, but switching to Serial instead of Software Serial. Maybe that's what was causing all the noise.

10

My thinking is that the interrupts used by software serial and those used by Servo.h are interferring with each other. When in each isr, the other interrupts are queued rather than executed.

11

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.