Trying to control 3 motors with packet serial
Posted: 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();
// */
}
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();
// */
}