Hi All
I'm working on a project similar to the one shown in this video :
and I would like to add this Processing program to the main program as shown on the link below :
so I can at the same time stabilize the platform and display the angle values on the screen.
I tried adding them together but with no results. Can you please help me out
here's the main program that stabilizes the platform :
#include <Wire.h>
#include "Kalman.h"
#include <Servo.h>
Servo servo1;
Servo servo2;
Kalman kalmanX;
Kalman kalmanY;
const uint8_t IMUAddress = 0x68;
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;
uint32_t timer;
void setup() {
servo1.attach(6);
servo2.attach(9);
Serial.begin(115200);
Wire.begin();
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);
}
kalmanX.setAngle(180); // Set starting angle
kalmanY.setAngle(180);
timer = micros();
}
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]);
gyroX = ((data[8] << 8) | data[9]);
gyroY = ((data[10] << 8) | data[11]);
gyroZ = ((data[12] << 8) | data[13]);
/* 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;
double gyroXrate = (double)gyroX/131.0;
double gyroYrate = -((double)gyroY/131.0);
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter
gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
//gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
timer = micros();
servo1.write(kalAngleX/2.0);
servo2.write(kalAngleY/2.0);
temp = ((double)tempRaw + 12412.0) / 340.0;
/* Print Data */
/*
Serial.print(accX);Serial.print("\t");
Serial.print(accY);Serial.print("\t");
Serial.print(accZ);Serial.print("\t");
Serial.print(gyroX);Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ);Serial.print("\t");
*/
Serial.print(accXangle);Serial.print("\t");
Serial.print(accYangle);Serial.print("\t");
Serial.print(gyroXangle);Serial.print("\t");
Serial.print(gyroYangle);Serial.print("\t");
Serial.print(compAngleX);Serial.print("\t");
Serial.print(compAngleY); Serial.print("\t");
Serial.print(kalAngleX);Serial.print("\t");
Serial.print(kalAngleY);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;
}
the Processing code can be found here :
http://www.varesano.net/blog/fabio/initial-tests-freeimu-v04-and-mpu6050
Abdelali:
I tried adding them together but with no results.
What does that mean? Did it compile? Did it run? Did it do what you intended? If not, what did you intend and what did it actually do?
You have to figure out what the Processing sketch is expecting for input and change your Arduino sketch to produce that output.
I went through that process but I couldn't get any results . pleaaaase if somebody could help I'll be so greatful
What you can do is to write a Processing sketch to read Serial input and parse the numbers that Arduino prints out and then displays the numbers on screen. Do you know how to parse numbers from a serial text stream? That is the key here.
Hi Dear liudr
Sincerly I really don't know how to parse numbers from a serial text stream, I'm greatful if you could help me out here
This is how you print the data out from Arduino:
Serial.print(accXangle);Serial.print("\t");
Serial.print(accYangle);Serial.print("\t");
Serial.print(gyroXangle);Serial.print("\t");
Serial.print(gyroYangle);Serial.print("\t");
Serial.print(compAngleX);Serial.print("\t");
Serial.print(compAngleY); Serial.print("\t");
Serial.print(kalAngleX);Serial.print("\t");
Serial.print(kalAngleY);Serial.print("\t");
Serial.print("\n");
You should be able to get data from Processing using this format. I have not used Processing since shortly after they rolled out 1.0
Here is a sample program I wrote:
import processing.serial.*;
Serial myPort; // The serial port
void setup()
{
size(220, 220);
Object[] possibilities = Serial.list();
myPort = new Serial(this, (String)possibilities[possibilities.length-1], 115200);
}
void draw()
{
char [] inBuffer=new char[256];
char write_pointer=0;
while (true)
{
if (myPort.available() > 0)
{
inBuffer[write_pointer]=myPort.readChar();
if (inBuffer[write_pointer]=='\n')
{
inBuffer[write_pointer]='\0';
break;
}
write_pointer++;
}
}
//Interpret the buffer content with an sscanf
String inString=new String(inBuffer);
float[] inNums = float(split(inString,'\t'));
for (int i=0;i<8;i++)
{
print(inNums[i]);
print('\t');
}
while(true){}
}
Give this a try.