Главная » Форумы » Проекты

Система стабилизации в движении

6 ответов [Последнее сообщение]
Чт, 21/06/2018 - 10:29
Zbigen
Offline
Зарегистрирован: 21.06.2018

Всем привет!

Есть модуль GY86. Задача - управлять коллекторным мотором через bts7960 при отклонении модуля по одной оси.

Нашел код работы с MPU6050. При заливе первого кода получаю информацию с модуля MPU6050 вида: 359.86 358.09 30.01

Есть код работы с мотором от потенциометра. При заливе второго кода спокойно управляю мотором от потенциометра.
 
Теперь хочу вместо потенциометра использовать значение accXangle.
Топорным способом объединить два кода не получилось. 
Прошу совета, возможно ли объединить эти два кода, или нет.
 
Код 1.
 
#include <Wire.h>
 
int LPWM_Output = 6; 
int RPWM_Output = 5; 
uint8_t IMUAddress = 0x68;
 
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
 
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
 
void setup() {  
  Serial.begin(115200);
  Wire.begin();  
  pinMode(RPWM_Output, OUTPUT);
  pinMode(LPWM_Output, OUTPUT);
  i2cWrite(0x6B,0x00); // Disable sleep mode  
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  }  
}
 
void loop() {
  /* Update all the values */
  uint8_t* data = i2cRead(0x3B,14);  
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);  
  tempRaw = ((data[6] << 8) | data[7]);  
  
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
 
  temp = ((double)tempRaw + 12412.0) / 340.0;
 
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(accYangle);Serial.print("\t"); 
 
  //Serial.print(reversePWM);Serial.print("\t"); 
  
  Serial.print(temp);Serial.print("\t");
   
  Serial.print("\n");
  
  delay(1); // The accelerometer's maximum samples rate is 1kHz
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];  
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
} 
 
Код 2.
 
// контакт Arduino для подключения потенциометра
int SENSOR_PIN = 0; 
// контакт Arduino для подключения к RPWM модуля BTS7960B
int RPWM_Output = 5; 
// контакт Arduino для подключения к LPWM модуля BTS7960B
int LPWM_Output = 6; 
 
void setup() {
pinMode(RPWM_Output, OUTPUT);
pinMode(LPWM_Output, OUTPUT);
}
void loop() {
int sensorValue = analogRead(SENSOR_PIN);
// средняя точка показаний потенциометра
// раздел направления вращения мотора
if (sensorValue < 400) {
// вращение в противоположную сторону
// 0-255
int reversePWM = (512-sensorValue) / 2;
analogWrite(LPWM_Output, 0);
analogWrite(RPWM_Output, reversePWM);}
else {
// вращение одну сторону
// 0-255
int forwardPWM = (sensorValue - 512) / 2;
analogWrite(LPWM_Output, forwardPWM);
analogWrite(RPWM_Output, 0);
}
}
 
Попытка объединения:
#include <Wire.h>
 
int LPWM_Output = 6; 
int RPWM_Output = 5; 
uint8_t IMUAddress = 0x68;
 
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
 
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
 
void setup() {  
  Serial.begin(115200);
  Wire.begin();  
  pinMode(RPWM_Output, OUTPUT);
  pinMode(LPWM_Output, OUTPUT);
  i2cWrite(0x6B,0x00); // Disable sleep mode  
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  }  
}
 
void loop() {
  /* Update all the values */
  uint8_t* data = i2cRead(0x3B,14);  
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);  
  tempRaw = ((data[6] << 8) | data[7]);  
  
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
 
if (5 < accXangle < 10) {
// вращение в противоположную сторону
// 0-255
// int reversePWM = 255;
analogWrite(LPWM_Output, 0);
analogWrite(RPWM_Output, 255);}
 
  temp = ((double)tempRaw + 12412.0) / 340.0;
 
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(accYangle);Serial.print("\t"); 
 
  //Serial.print(reversePWM);Serial.print("\t"); 
  
  Serial.print(temp);Serial.print("\t");
   
  Serial.print("\n");
  
  delay(1); // The accelerometer's maximum samples rate is 1kHz
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];  
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
} 
При заливе первого кода получаю информацию с модуля MPU6050 вида: 359.86 358.09 30.01
При заливе второго кода спокойно управляю мотором от потенциометра.
 
Теперь хочу вместо потенциометра использовать значение accXangle.
Топорным способом объединить два кода не получилось. 
Прошу совета, возможно ли объединить эти два кода, или нет.
Верх
Чт, 21/06/2018 - 10:32
#1
DetSimen
DetSimen аватар
Offline
Зарегистрирован: 25.01.2017

Zbigen пишет:

Прошу совета, возможно ли объединить эти два кода, или нет.

 
Возможно. 
Верх
Чт, 21/06/2018 - 11:18
#2
Zbigen
Offline
Зарегистрирован: 21.06.2018

Спасибо!

Можете подсказать - как именно это сделать? )

Верх
Чт, 21/06/2018 - 11:22
#3
DetSimen
DetSimen аватар
Offline
Зарегистрирован: 25.01.2017

Нет.  Разбираца надо вдумчиво.  А у меня пока синдром. 

Верх
Чт, 21/06/2018 - 11:57
#4
sadman41
Offline
Зарегистрирован: 19.10.2016

Интересно, зачем каждый раз вот эта прелюдия устраивается? Возможно ли, невозможно ли... В конце один черт следует "А сделайте за меня, иначе я не успеваю".

Верх
Чт, 21/06/2018 - 12:03
#5
Zbigen
Offline
Зарегистрирован: 21.06.2018

Уже сделал)

Код:

#include <Wire.h>
#include <MPU6050.h>
 
#define first_motor_pin1 5
#define first_motor_pin2 6
 
int RPWM_Output = 5; 
int LPWM_Output = 6;
 
int SENSOR_PIN = 0; 
int s=0;
 
MPU6050 sensor;
int16_t ax, ay, az;
int16_t gx, gy, gz;
 
int first_motor_speed;
 
void setup ( )
{
  Wire.begin( );
  Serial.begin (9600);
 
  pinMode(RPWM_Output, OUTPUT);
  pinMode(LPWM_Output, OUTPUT);
  
  Serial.println ("Initializing the sensor");
  sensor.initialize ( );
  Serial.println(sensor.testConnection( ) ? "Successfully Connected" : "Connection failed");
  delay(1000);
  Serial.println("Taking Values from the sensor");
  delay(1000);
}
 
void loop ( )
{
  sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  ax = map(ax, -17000, 17000, -125, 125);
 
  int sensorValue = analogRead(SENSOR_PIN);
  //задаем параметр чувствительности
  s = sensorValue/50;
  
  if (ax > s) {
  analogWrite(LPWM_Output, 255-ax);
  analogWrite(RPWM_Output, 0);}
  else if (ax < -s) {
  analogWrite(LPWM_Output, 0);
  analogWrite(RPWM_Output, 255+ax);
  }
  else {
  analogWrite(LPWM_Output, 0);
  analogWrite(RPWM_Output, 0);
  }
  
  //Serial.print ("Motor1 Speed = ");
  //Serial.println (LPWM_Output, DEC);
  //Serial.print (" && ");
  //Serial.print ("Motor2 Speed = ");
  //Serial.println (second_motor_speed, DEC);
   
  //analogWrite (first_motor_pin2, first_motor_speed);
  //analogWrite (second_motor_pin2, second_motor_speed);
  delay (200);
}
Верх
Чт, 21/06/2018 - 12:20
#6
ЕвгенийП
ЕвгенийП аватар
Offline
Зарегистрирован: 25.05.2015

Zbigen пишет:

Уже сделал)

Молодец!

Теперь ещё научитесь код правильно вставлять, а то (открою Вам секрет) Ваш код собственно никто и не читал.

Верх