Trying to control 3 motors with packet serial

Questions about using encoders with the Roboclaw product line
Post Reply
laptophead
Posts: 9
Joined: Mon Oct 16, 2017 5:03 pm

Trying to control 3 motors with packet serial

Post by laptophead » Mon Oct 16, 2017 5:30 pm

I was succesfull in moving all 3 a bit, so I'm pretty sure the hardware works. I have done all the PID setup for each of the 2 robotclaws.

Now the base motor only goes fwd and not coming back. the rest the same, they behave erratically...
I am new at this, please help. I am using a teensy 3.6

here is my arduino code (Why cant I attach a .ino file?)


#include "RoboClaw.h"

// encoder section
#include "AltEncoder.h"
using namespace AltEncoder;

Encoder KnobEncoder(34, 33); // Control knob for testing
Encoder BaseRotEncoder(12, 11); // Encoder for base Rotation
Encoder ShoulderEncoder(9, 10); //Encoder for Shoulder
Encoder ElbowEncoder(7, 8); // Elbow

Encoder* encoderList[] =
{
&KnobEncoder,
&BaseRotEncoder,
&ShoulderEncoder,
&ElbowEncoder,
nullptr
};


int KnobEncoderVal, BaseRotEncoderVal, ShoulderEncoderVal, ElbowEncoderVal ;


// breaks Section
#define BaseBrakePin 21
#define ShoulderBrakePin 22
#define ElbowBrakePin 23


#include <Keypad.h>

const byte ROWS = 4; //four rows
const byte COLS = 4; //four columns


char hexaKeys[ROWS][COLS] = {
{'1', '2', '3', 'A'},
{'4', '5', '6', 'B'},
{'7', '8', '9', 'C'},
{'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {25, 26, 27, 28}; //connect to the row pinouts of the keypad
byte colPins[COLS] = {29, 30, 31, 32}; //connect to the column pinouts of the keypad

boolean Drive = false;
boolean Cancel= false;

Keypad customKeypad = Keypad( makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS);
RoboClaw roboclaw(&Serial1, 10000);

#define adr128 0x80 // first dual H Bridge controller
#define adr129 0x81

//Velocity PID coefficients
#define Kp 1.0
#define Ki 0.5
#define Kd 0.25
#define qpps 44000

void setup() {
//Open Serial and roboclaw serial ports
Serial.begin(57600);
roboclaw.begin(38400);
pinMode (13, OUTPUT);
pinMode (BaseBrakePin, OUTPUT);
pinMode (ShoulderBrakePin, OUTPUT);
pinMode (ElbowBrakePin, OUTPUT);


Controller::begin(encoderList, 25/*µs*/); // choose a sampling period which is at least a factor
ShoulderEncoder.counter = 0;
pinMode (34, INPUT_PULLUP); // for the knob readings
pinMode (33, INPUT_PULLUP);
digitalWrite (13, 1);


// pinMode (11, INPUT);
// pinMode (12, INPUT);

//Set PID Coefficients
roboclaw.SetM1VelocityPID(adr128, Kd, Kp, Ki, qpps);
roboclaw.SetM2VelocityPID(adr128, Kd, Kp, Ki, qpps);
roboclaw.SetM1VelocityPID(adr129, Kd, Kp, Ki, qpps);
roboclaw.SetM2VelocityPID(adr129, Kd, Kp, Ki, qpps);
}



void loop() {


KnobEncoderVal = KnobEncoder.counter;
BaseRotEncoderVal = BaseRotEncoder.counter;
ShoulderEncoderVal = ShoulderEncoder.counter;
ElbowEncoderVal = ElbowEncoder.counter;

/*
Serial.print(" KnobEncoderVal= ");
Serial.print(KnobEncoderVal);
Serial.print(" BaseRot = ");
Serial.print( BaseRotEncoderVal);
Serial.print(" Shoulder = ");
Serial.print( ShoulderEncoderVal);
Serial.print(" ElbowEnc = ");
Serial.print( ElbowEncoderVal);
Serial.println();
*/

// releasing brakes
digitalWrite (BaseBrakePin, 0);
digitalWrite (ShoulderBrakePin, 0); // 0 is released, 1 is brake applied
digitalWrite (ElbowBrakePin, 1);


//Drive4Motors();

char customKey = customKeypad.getKey();

//if (customKey) { }


Serial.println(customKey);
//Serial.println(Drive);

if (customKey == 'D')
{

Drive4Motors();

}


delay (10);
}

void Drive4Motors()
{ uint8_t depth1, depth2, depth3, depth4;

roboclaw.SpeedAccelDistanceM1(adr128, 0, 0, 0, 1); // Grip
roboclaw.SpeedAccelDistanceM2(adr128, 12000, 12000, 0, 1); // Shoulder
roboclaw.SpeedAccelDistanceM1(adr129, 12000, 12000, 8200, 1); //Base
roboclaw.SpeedAccelDistanceM2(adr129, 12000, 12000, 0, 1); // Elbow

// 1 AT THE END MEANS EXECUTE IMMEDIATLY THE NEXT COMMAND. 0 MEANS WAYIT TILL THE FIRST TAKS IS FINISHED
do {
displayspeed129();
roboclaw.ReadBuffers(adr128, depth1, depth2);
roboclaw.ReadBuffers(adr129, depth3, depth4);
} while (depth1 != 0x80 && depth2 != 0x80 && depth3 != 0x80 && depth4 != 0x80); //loop until distance command completes

delay(2000);

roboclaw.SpeedAccelDistanceM1(adr128, 0, 0, 0, 1);// Grip
roboclaw.SpeedAccelDistanceM2(adr128, 12000, -12000, 0, 1); // Shoulder
roboclaw.SpeedAccelDistanceM1(adr129, 12000, -12000, 8200, 1); // Base
roboclaw.SpeedAccelDistanceM2(adr129, 12000, -12000, 0, 1); // Elbow

do {
displayspeed129();
roboclaw.ReadBuffers(adr128, depth1, depth2);
roboclaw.ReadBuffers(adr129, depth3, depth4);
} while (depth1 != 0x80 && depth2 != 0x80 && depth3 != 0x80 && depth4 != 0x80); //loop until distance command completes

//delay(2000);
}






void displayspeed128(void)

{
uint8_t status1, status2, status3, status4;
bool valid1, valid2, valid3, valid4;

int32_t enc1 = roboclaw.ReadEncM1(adr128, &status1, &valid1);
int32_t enc2 = roboclaw.ReadEncM2(adr128, &status2, &valid2);
int32_t speed1 = roboclaw.ReadSpeedM1(adr128, &status3, &valid3);
int32_t speed2 = roboclaw.ReadSpeedM2(adr128, &status4, &valid4);
Serial.print("Encoder1:");
if (valid1) {
Serial.print(enc1, DEC);
Serial.print(" ");
Serial.print(status1, HEX);
Serial.print(" ");
}
else {
Serial.print("failed ");
}
Serial.print("Encoder2:");
if (valid2) {
Serial.print(enc2, DEC);
Serial.print(" ");
Serial.print(status2, HEX);
Serial.println(" ");
}
else {
Serial.println("failed ");
}

///*
Serial.print("Speed1:");
if (valid3) {
Serial.print(speed1, DEC);
Serial.print(" ");
}
else {
Serial.print("failed ");
}
Serial.print("Speed2:");
if (valid4) {
Serial.print(speed2, DEC);
Serial.print(" ");
}
else {
Serial.print("failed ");
}
Serial.println();
// */
}


void displayspeed129(void)
{
uint8_t status1, status2, status3, status4;
bool valid1, valid2, valid3, valid4;

int32_t enc1 = roboclaw.ReadEncM1(adr129, &status1, &valid1);
int32_t enc2 = roboclaw.ReadEncM2(adr129, &status2, &valid2);
int32_t speed1 = roboclaw.ReadSpeedM1(adr129, &status3, &valid3);
int32_t speed2 = roboclaw.ReadSpeedM2(adr129, &status4, &valid4);
Serial.print("BaseEnc:");
if (valid1) {
Serial.print(enc1, DEC);
Serial.print(" ");
Serial.print(status1, HEX);
Serial.print(" ");
}
else {
Serial.print("failed ");
}
Serial.print("ElbowEnc:");
if (valid2) {
Serial.print(enc2, DEC);
Serial.print(" ");
Serial.print(status2, HEX);
Serial.println(" ");
}
else {
Serial.println("failed ");
}

///*
Serial.print("BaseEnc:");
if (valid3) {
Serial.print(speed1, DEC);
Serial.print(" ");
}
else {
Serial.print("failed ");
}
Serial.print("ElbowEnc:");
if (valid4) {
Serial.print(speed2, DEC);
Serial.print(" ");
}
else {
Serial.print("failed ");
}
Serial.println();
// */
}

User avatar
Basicmicro Support
Posts: 1247
Joined: Thu Feb 26, 2015 9:45 pm

Re: Trying to control 3 motors with packet serial

Post by Basicmicro Support » Tue Oct 17, 2017 9:41 am

1. Setup and test the PID settings using IonStudio and save the settings to each Roboclaw.
2. Remove the PID setup code in your arduino example. The default PID constants are just examples.
3. If you still have problems you need to send much more simpified program that shows the problem. Should need more than around 30 lines of code to reproduce the problem.

laptophead
Posts: 9
Joined: Mon Oct 16, 2017 5:03 pm

Re: Trying to control 3 motors with packet serial

Post by laptophead » Tue Oct 17, 2017 2:30 pm

OK I simplified to two motors and it works.

The issues:
1. The motor does not come back to the place (number of pulses ) it came from. Hundreds of pulses apart.
I though if it is a PID, it wont stop till the error is 0. Might be slow or overshoot, but it should get there.

2. How do I read the existing PID settings in the Robotclaw now?
How do I convert them in the arduino sketch kind of numbers so I can increase -decrease them.




//Includes required to use Roboclaw library
#include <SoftwareSerial.h>
#include "RoboClaw.h"

//See limitations of Arduino SoftwareSerial

RoboClaw roboclaw(&Serial1, 10000);

#define address 0x81

//Velocity PID coefficients
#define Kp 1.0
#define Ki 0.5
#define Kd 0.25
#define qpps 44000

#define BaseBrakePin 21
#define ShoulderBrakePin 22
#define ElbowBrakePin 23

void setup() {
//Open Serial and roboclaw serial ports
Serial.begin(57600);
roboclaw.begin(38400);

//Set PID Coefficients
//roboclaw.SetM1VelocityPID(address,Kd,Kp,Ki,qpps);
//roboclaw.SetM2VelocityPID(address,Kd,Kp,Ki,qpps);

pinMode (BaseBrakePin, OUTPUT);
pinMode (ShoulderBrakePin, OUTPUT);
pinMode (ElbowBrakePin, OUTPUT);
digitalWrite (BaseBrakePin, 0);
digitalWrite (ShoulderBrakePin, 1); // 0 is released, 1 is brake applied
digitalWrite (ElbowBrakePin, 0);
}



void loop() {
MoveNow ();


}

void MoveNow ()

{ uint8_t depth1,depth2;

roboclaw.SpeedAccelDistanceM1(address,12000,12000,34200,1);// base
roboclaw.SpeedAccelDistanceM2(address,12000,-12000,3000,1); // elbow
roboclaw.SpeedAccelDistanceM1(address,12000,0,0); //distance traveled is v*v/2a = 12000*12000/2*12000 = 6000
roboclaw.SpeedAccelDistanceM2(address,12000,0,0); //that makes the total move in ondirection 48000
do{
displayspeed();
roboclaw.ReadBuffers(address,depth1,depth2);
}while(depth1!=0x80 && depth2!=0x80); //loop until distance command completes

delay(3000);

roboclaw.SpeedAccelDistanceM1(address,12000,-12000,34200,1);
roboclaw.SpeedAccelDistanceM2(address,12000,12000,3000,1);
roboclaw.SpeedAccelDistanceM1(address,12000,0,0);
roboclaw.SpeedAccelDistanceM2(address,12000,0,0);
do{
displayspeed();
roboclaw.ReadBuffers(address,depth1,depth2);
}while(depth1!=0x80 && depth2!=0x80); //loop until distance command completes

delay(3000);
}

void displayspeed(void)
{
uint8_t status1,status2,status3,status4;
bool valid1,valid2,valid3,valid4;

int32_t enc1= roboclaw.ReadEncM1(address, &status1, &valid1);
int32_t enc2 = roboclaw.ReadEncM2(address, &status2, &valid2);
int32_t speed1 = roboclaw.ReadSpeedM1(address, &status3, &valid3);
int32_t speed2 = roboclaw.ReadSpeedM2(address, &status4, &valid4);
Serial.print("Base_Enc1:");
if(valid1){
Serial.print(enc1,DEC);
Serial.print(" ");
Serial.print(status1,HEX);
Serial.print(" ");
}
else{
Serial.print("failed ");
}
Serial.print("Elbow_Enc2:");
if(valid2){
Serial.print(enc2,DEC);
Serial.print(" ");
Serial.print(status2,HEX);
Serial.print(" ");
}
else{
Serial.print("failed ");
}
Serial.print("Speed1:");
if(valid3){
Serial.print(speed1,DEC);
Serial.print(" ");
}
else{
Serial.print("failed ");
}
Serial.print("Speed2:");
if(valid4){
Serial.print(speed2,DEC);
Serial.print(" ");
}
else{
Serial.print("failed ");
}
Serial.println();
}

User avatar
Basicmicro Support
Posts: 1247
Joined: Thu Feb 26, 2015 9:45 pm

Re: Trying to control 3 motors with packet serial

Post by Basicmicro Support » Wed Oct 18, 2017 9:07 am

As I said in my first reply, you need to tune the PID settings using Ion Studio. I recommend you use the autotuner to get the initial PID values. Then you can save them to the Roboclaw from Ion Studio. You will not need to set them in your Arduino sketch at all. They will be loaded automatically when the board powers up.

Based on your description your PID values are not set close to where they need to be. PID isnt magic. You have to have proper values for your motors. The examples use 1, .5 and .25 as place holders. These will hardly even move most motors let alone come close to the correct speed or position targets.

Post Reply