Hi, I'm working on a self balancing robot but I'm having a bunch of different problems!
I'm using a Seeeduino XIAO.
The step motors(nema 17) are operating too weak and not smooth at all.
I can't get the PID library set up in a way that it returns good functional numbers to the OUTPUT variable in order to drive the motors.
This is my code:
#include<Wire.h>
#include <AccelStepper.h>
#include <PID_v1.h>
//MOTORS STUFF//////////////////////////////////////////////////////////////////////////////////////////////
// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver:
#define dirPinL 2
#define stepPinL 3
#define dirPinR 0
#define stepPinR 1
#define motorInterfaceType 1
// Creates a Motor instance
AccelStepper myStepperR(motorInterfaceType, stepPinR, dirPinR);
// Creates an instance
AccelStepper myStepperL(motorInterfaceType, stepPinL, dirPinL);
bool down, direcao;
int rSpeed, lSpeed;
//MPU SETUP//////////////////////////////////////////////////////////////////////////////////////////////
const int MPU_addr=0x68; int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int minVal=265;
int maxVal=402;
double x; double y; double z;
long previousMillis = millis(); //Contador do tempo para ler MPU
long interval = 15; //time for motor to swict direction
//PID SETUP/////////////////////////////////////////////////////////////////////////////////////////////////
double setpoint = 0; // Define qual posicao o robo vai tentar manter
float Kp = 35; // (P)roportional Tuning Parameter (20)
float Ki = 0; // (I)ntegral Tuning Parameter (70)
float Kd = 0; // (D)erivative Tuning Parameter
float lastTime = 0; // Records the time the function was last called
double output = 0;
PID PID1(&y/*imput value*/, &output, &setpoint, Kp, Ki, Kd, DIRECT);
void setup() {
down = true; //comeca assumindo que o robo esta caido
//Preparando comunicacao I2C//////////////////////////////////////////////////////////////////////////////////////////////
//MPU SETUP
Wire.begin();
Wire.setTimeout(2000);
Wire.setClock(400000);
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
SerialUSB.begin(9600);
previousMillis = millis();
//Motors SETUP//////////////////////////////////////////////////////////////////////////////////////////////
// set the maximum speed, acceleration factor,
// initial speed and the target position
pinMode(dirPinL, OUTPUT); // Left Motor Direction
pinMode(dirPinR, OUTPUT); // Left Motor Direction
digitalWrite(dirPinL, LOW);
digitalWrite(dirPinR, HIGH);
myStepperR.setMaxSpeed(10000);
myStepperR.setAcceleration(100);
myStepperR.setSpeed(10);
myStepperR.moveTo(2000);
myStepperL.setMaxSpeed(10000);
myStepperL.setAcceleration(100);
myStepperL.setSpeed(100);
myStepperL.moveTo(2000);
delay(3000);
//PID SETUP//////////////////////////////////////////////////////////////////////////////////////////
PID1.SetMode(AUTOMATIC);
PID1.SetOutputLimits(-7000,7000);
PID1.SetSampleTime(20);
}//end setup///
void loop() {
previousMillis = millis();
readMPU();
if(y>-4&&y<4){
y=0;
output=0;
}else{
PID1.Compute();
//output = myPID.step(setpoint, y);
}
Serial.print("y = ");
Serial.print(y);
Serial.print(" | output = ");
Serial.print(output);
Serial.print(" | direcao = ");
Serial.println(direcao);
if(down==false){
while((millis()-previousMillis)<interval){
myStepperR.setSpeed(output);
myStepperL.setSpeed(output*(-1));
myStepperR.runSpeed();
myStepperL.runSpeed();
}/*end while*/
}else{
Serial.println("DOWN");
//Serial.println(y);
//emd if down false
}
}//endvoidloop
void readMPU(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
//y = yAng;
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
Serial.print(" RAW y = ");
Serial.print(y);
Serial.print(" | ");
//delay(1000);
//REMAP AND FORMAT Y
if((y<45)&&(y>0)){
y=map(y,45,0,300,0);//para tras
y=y*(-1);
down =false;
direcao = false; //para tras
digitalWrite(dirPinL, LOW);
digitalWrite(dirPinR, HIGH);
}else if((y<361)&&(y>321)){
y=map(y,359,320,0,300);
down =false;
direcao = true; //pata frente
digitalWrite(dirPinL, HIGH);
digitalWrite(dirPinR, LOW);
}else{
down =true;
y=0;
}
/**/
//y=map(y,0,180,-90,90);
}
I'm pretty lost at this point and any help would be very welcomed!
Thank you!
