Bonjour, je bosse sur un robot de self-balancing suivant ce tutoriel instructable
Pour un potentiel usage WIFI avec un Arduino nano 33 IoT, j'ai changé le microcontroleur de arduino mini à arduino nano. J'ai aussi changé le controleur moteur (simple erreur de commande), le DRV8833 devient un SparkFun TB6612.
Le problème que j'ai est que quand l'upload le sketch complet du robot ci-dessous, j'arrive à apercevoir une fluctuation de la vitesse en fonction de l'inclinaison cependant au-bout d'un certaint temps, le moniteur série n'affiche plus les nouvelles vitesse et les moteurs sont bloqué à la dernière vitesse affiché
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <SparkFun_TB6612.h>
#include <SoftwareSerial.h>
int count = 0;
// ===== MOTOR CONFIGURATION =====
// Configuration des sens de rotation des moteurs (1 ou -1)
const int offsetA = 1;
const int offsetB = -1;
// Ports numériques associés aux entrées de contrôle (les entrées PWM doivent être connectées à des ports PWM de l'Arduino)
#define AIN1 4
#define PWMA 5
#define BIN2 6
#define BIN1 7
#define STBY 8
#define AIN2 10
#define PWMB 11
volatile int motorPower = 200;
// Initialisation des moteurs
Motor motorA = Motor(AIN1, AIN2, PWMA, offsetA, STBY);
Motor motorB = Motor(BIN1, BIN2, PWMB, offsetB, STBY);
// ===== US sensor CONFIGURATION =====
// Constate for US sensor
#define TRIGGER_PIN 3
#define ECHO_PIN 2
#define MAX_DISTANCE 75
SoftwareSerial mySerial(TRIGGER_PIN, ECHO_PIN);
unsigned int HighByte = 0;
unsigned int LowByte = 0;
unsigned int distancemm;
int distanceCm;
// ===== MPU6050 CONFIGURATION =====
// Constate for acelerometer and gyroscope
#define Kp 50
#define Kd 0
#define Ki 0
#define sampleTime 0.005
float targetAngle = 3;
MPU6050 mpu;
int16_t accX, accZ, gyroY;
volatile int gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
// ===== LED CONFIGURATION =====
// Led pin
#define Greenled 12
#define Redled 13
// Flashing of leds at initialization
void ledFlashing(int led[], int length){
Serial.println("[LEDFLASHING] led flashing ...");
for(int i = 0; i < length; i++){
digitalWrite(led[i], HIGH);
}
delay(500);
for(int i = 0; i < length; i++){
digitalWrite(led[i], LOW);
}
delay(500);
for(int i = 0; i < length; i++){
digitalWrite(led[i], HIGH);
}
delay(500);
for(int i = 0; i < length; i++){
digitalWrite(led[i], LOW);
}
}
// set motors speed
void setMotors(int leftMotorSpeed, int rightMotorSpeed) {
// Serial.print("speed left and right : \t");
// Serial.print(leftMotorSpeed);
// Serial.print("\t");
// Serial.println(rightMotorSpeed);
motorA.drive(leftMotorSpeed);
motorB.drive(rightMotorSpeed);
}
void init_PID() {
// initialize Timer1
cli(); // disable global interrupts
TCCR1A = 0; // set entire TCCR1A register to 0
TCCR1B = 0; // same for TCCR1B
// set compare match register to set sample time 5ms
OCR1A = 9999;
// turn on CTC mode
TCCR1B |= (1 << WGM12);
// Set CS11 bit for prescaling by 8
TCCR1B |= (1 << CS11);
// enable timer compare interrupt
TIMSK1 |= (1 << OCIE1A);
sei(); // enable global interrupts
}
void setup() {
mySerial.begin(9600);
Serial.begin(9600);
Serial.println(" === CASPER v2 ===");
Serial.println("[SETUP] begin ...");
// set the status LED to output mode
pinMode(Greenled, OUTPUT); // Blue flashing
pinMode(Redled, OUTPUT); // Red obstacle detector
// flashing of led
int led_pin[2] = {Greenled, Redled};
ledFlashing(led_pin, 2);
// initialize the MPU6050 and set offset values
mpu.initialize();
Serial.println(F("[SETUP] Testing device connections..."));
Serial.println(mpu.testConnection() ? F("[SETUP] MPU6050 connection successful") : F("[SETUP] MPU6050 connection failed"));
Serial.println("[SETUP] Yaccel");
mpu.setYAccelOffset(1593);
Serial.println("[SETUP] Zaccel");
mpu.setZAccelOffset(963);
Serial.println("[SETUP] XGyro");
mpu.setXGyroOffset(40);
// initialize PID sampling loop
Serial.println("[SETUP] init pid");
init_PID();
Serial.println("[SETUP] done ...");
}
void loop() {
// read acceleration and gyroscope values
accX = mpu.getAccelerationX();
accZ = mpu.getAccelerationZ();
gyroY = mpu.getRotationY();
// set motor power after constraining it
motorPower = constrain(motorPower, -255, 255);
Serial.println(motorPower);
setMotors(motorPower, motorPower);
// measure distance every 100 milliseconds
if((count%20) == 0){
mySerial.flush();
mySerial.write(0X55); // trig US-100 begin to measure the distance
if (mySerial.available() >= 2) // check receive 2 bytes correctly
{
HighByte = mySerial.read();
LowByte = mySerial.read();
distancemm = HighByte * 256 + LowByte; // Calculate the distance
if ((distancemm > 1) && (distancemm < 10000)) {
distanceCm = distancemm/10;
}
}
}
// if((distanceCm < 20) && (distanceCm != 0)) {
// digitalWrite(Redled, HIGH);
// targetAngle = 42.5;
// }
// else {
// digitalWrite(Redled, LOW);
// targetAngle = 40.75;
// }
}
// The ISR will be called every 5 milliseconds
ISR(TIMER1_COMPA_vect)
{
// calculate the angle of inclination
accAngle = atan2(accX, accZ)*RAD_TO_DEG;
gyroRate = map(gyroY, -32768, 32767, -250, 250);
gyroAngle = (float)gyroRate*sampleTime;
currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
error = currentAngle - targetAngle;
errorSum = errorSum + error;
errorSum = constrain(errorSum, -300, 300);
//calculate output from P, I and D values
motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
prevAngle = currentAngle;
// toggle the led on pin13 every second
count++;
if(count == 200) {
count = 0;
digitalWrite(Greenled, !digitalRead(Greenled));
}
}
Je vous joint aussi le schéma de montage
J'avoue être franchement à court de pistes
Merci D'avance
Breizh