Avans/Controller
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Rewrote mpu6050 code, better and stable results now. Added rumble

Browse Source
This commit is contained in:
jancoow
2016-06-02 22:20:07 +02:00
parent c5990d4fc0
commit bdd86ac036
1 changed files with 123 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Controller/Controller.ino
+123 -123
View File
@@ -1,5 +1,5 @@
#include "I2Cdev.h" #include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h" #include "MPU6050.h"
#include <Wire.h> #include <Wire.h>
#include <WiFiUdp.h> #include <WiFiUdp.h>
@@ -32,25 +32,22 @@ struct SwitchData{
//mpu6050 //mpu6050
MPU6050 mpu; MPU6050 mpu;
bool dmpReady = false; // set true if DMP init was successful int16_t ax, ay, az;
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU int16_t gx, gy, gz;
uint16_t packetSize; // expected DMP packet size (default is 42 bytes) double ax_scaled, ay_scaled, az_scaled;
uint16_t fifoCount; // count of all bytes currently in FIFO double gx_scaled, gy_scaled, gz_scaled;
uint8_t fifoBuffer[64]; // FIFO storage buffer double roll, pitch, yaw;
double gx_off, gy_off, gz_off;
// orientation/motion vars double gyro_scale = 131.0;
Quaternion q; // [w, x, y, z] quaternion container double accel_scale = 16384.0;
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
//Joystick offsets //Joystick offsets
int joystickoffset_x, joystickoffset_y; int joystickoffset_x, joystickoffset_y;
//rumble
boolean rumbleActivated;
u_long rumbleDuration;
//Data buffer for sensors //Data buffer for sensors
struct MPU6050Data mpu6050data = {0}; struct MPU6050Data mpu6050data = {0};
struct JoystickData joystickdata = {0}; struct JoystickData joystickdata = {0};
@@ -58,8 +55,8 @@ struct SwitchData switchdata = {0};
//UDP server (receiving) setup //UDP server (receiving) setup
unsigned int udpPort = 2730; unsigned int udpPort = 2730;
byte packetBuffer[512]; //udp package buffer byte packetBuffer[50]; //udp package buffer
char sendBuffer[45]; char sendBuffer[50];
WiFiUDP Udp; WiFiUDP Udp;
void setup(void){ void setup(void){
@@ -74,17 +71,21 @@ void setup(void){
Wire.begin(); Wire.begin();
//Magnetic sensor //Magnetic sensor
pinMode(13,INPUT); pinMode(12,INPUT);
//Rumble motor
pinMode(15, OUTPUT);
//clearEeprom(); //clearEeprom();
//connectLastBasestation(); connectLastBasestation();
searchBasestation(); //searchBasestation();
delay(1000); delay(1000);
ads.begin(); ads.begin();
joystickoffset_x = ads.readADC_SingleEnded(0); joystickoffset_x = ads.readADC_SingleEnded(0, 1);
joystickoffset_y = ads.readADC_SingleEnded(1); joystickoffset_y = ads.readADC_SingleEnded(1, 1);
mpu6050setup(); mpu6050setup();
} }
@@ -92,126 +93,125 @@ void setup(void){
void mpu6050setup(){ void mpu6050setup(){
mpu.initialize(); mpu.initialize();
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
int devStatus = mpu.dmpInitialize(); calibrate();
}
void calibrate(){
getMotion6Scaled();
get_x_rotation(ax_scaled, ay_scaled, az_scaled, &roll);
get_y_rotation(ax_scaled, ay_scaled, az_scaled, &pitch);
yaw = 0;
mpu.setXGyroOffset(62); gx_off = gx_scaled;
mpu.setYGyroOffset(25); gy_off = gy_scaled;
mpu.setZGyroOffset(-11); gz_off = gz_scaled;
mpu.setXAccelOffset(-2030);
mpu.setYAccelOffset(11);
mpu.setZAccelOffset(2119); // 1688 factory default for my test chip
if (devStatus == 0) { }
mpu.setDMPEnabled(true);
attachInterrupt(15, dmpDataReady, RISING); //intrupt pin on pin 15 void getMotion6Scaled(){
mpuIntStatus = mpu.getIntStatus(); mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ax_scaled = ax / accel_scale;
ay_scaled = ay / accel_scale;
az_scaled = az / accel_scale;
gx_scaled = gx / gyro_scale;
gy_scaled = gy / gyro_scale;
gz_scaled = gz / gyro_scale;
}
dmpReady = true; void get_y_rotation(double x, double y, double z, double* y_rotation){
*y_rotation = ((atan2(x, dist(y, z))) * 4068.0) / 71.0;
}
packetSize = mpu.dmpGetFIFOPacketSize(); void get_x_rotation(double x, double y, double z, double* x_rotation){
} else { *x_rotation = ((atan2(y, dist(x, z))) * -4068.0) / 71.0;
Serial.print(F("DMP Initialization failed (code ")); }
Serial.print(devStatus);
Serial.println(F(")")); double dist(double a, double b){
} return sqrt((a * a) + (b * b));
} }
u_long lasttime; u_long lasttime;
int count;
void loop(void){ void loop(void){
//wait for interupt from mpu6050, in the mean time pol udp server for new data readMpu6050(&mpu6050data);
while (!mpuInterrupt && fifoCount < packetSize) { if(millis() - lasttime > 30){
//receiving rumble or shock data from basestation lasttime = millis();
//Poll udp server
int noBytes = Udp.parsePacket(); int noBytes = Udp.parsePacket();
if ( noBytes ) { if ( noBytes ) {
Serial.print(millis() / 1000); Udp.read(packetBuffer,noBytes);
Serial.print(":Packet of "); if(packetBuffer[0] == '1'){
Serial.print(noBytes); char tmpstr[7];
Serial.print(" received from "); strncpy(tmpstr, (char*)packetBuffer+4, 6);
Serial.print(Udp.remoteIP()); int rumbleDuration = atoi(tmpstr);
Serial.print(":");
Serial.println(Udp.remotePort()); strncpy(tmpstr, (char*)packetBuffer+11, 3);
// We've received a packet, read the data from it int rumblePower = atoi(tmpstr);
Udp.read(packetBuffer,noBytes); // read the packet into the buffer
rumble(rumbleDuration, rumblePower);
// display the packet contents in HEX
for (int i=1;i<=noBytes;i++){ Serial.printf("Received udp package! Rumble duration: %d ~ Rumble power : %d", rumbleDuration, rumblePower);
Serial.print(packetBuffer[i-1],HEX); }else if(packetBuffer[0] == '2'){ //Shock
if (i % 32 == 0){
Serial.println(); }
}
else Serial.print(' ');
}
Serial.println(); Serial.println();
} }
delay(2);
} if(rumbleActivated == true && millis() > rumbleDuration){
rumbleActivated = false;
analogWrite(15, 0);
}
//We are out of the while loop, so mpu interupt fired; readJoystick(&joystickdata);
readFifoMpu6050(&mpu6050data);
readSwitches(&switchdata);
readJoystick(&joystickdata);
//Send new data
readSwitches(&switchdata); joystickdata.button = 0;
//Send new data sendUdpMessage(&joystickdata, &mpu6050data, &switchdata);
joystickdata.button = 0;
sendUdpMessage(&joystickdata, &mpu6050data, &switchdata);
if(millis() - lasttime > 1000){
printf("Samples per second: %d \n\r", count);
count = 0;
lasttime = millis();
}else{ }else{
count++; delay(6);
} }
}
void readJoystick(struct JoystickData *joystickdata){
joystickdata->x = ads.readADC_SingleEnded(1) - joystickoffset_x;
delay(8);
joystickdata->y = ads.readADC_SingleEnded(0) - joystickoffset_y;
} }
void readFifoMpu6050(struct MPU6050Data *mpu6050data){ void readMpu6050(struct MPU6050Data *mpu6050data){
// reset interrupt flag and get INT_STATUS byte getMotion6Scaled();
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & 0x02){
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// get Euler angles in degrees gx_scaled -= gx_off;
mpu.dmpGetQuaternion(&q, fifoBuffer); gy_scaled -= gy_off;
mpu.dmpGetGravity(&gravity, &q); gz_scaled -= gz_off;
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu6050data->yaw = ypr[0] * 18000/M_PI;
mpu6050data->pitch = ypr[1] * 18000/M_PI; double gx_delta = (gx_scaled * 0.01);
mpu6050data->roll = ypr[2] * 18000/M_PI; double gy_delta = (gy_scaled * 0.01);
printf("ypr: %d | %d | %d \n\r", mpu6050data->yaw, mpu6050data->pitch, mpu6050data->roll); double gz_delta = (gz_scaled * 0.01);
}
double rotationx, rotationy;
get_y_rotation(ax_scaled, ay_scaled, az_scaled, &rotationy);
get_x_rotation(ax_scaled, ay_scaled, az_scaled, &rotationx);
pitch = 0.98 * (pitch + gy_delta) + (0.02 * rotationy);
roll = 0.98 * (roll + gx_delta) + (0.02 * rotationx);
mpu6050data->pitch = pitch*100;
mpu6050data->roll = roll*100;
mpu6050data->yaw = 0;
}
void readJoystick(struct JoystickData *joystickdata){
joystickdata->x = ads.readADC_SingleEnded(1, 0) - joystickoffset_x;
delay(8);
joystickdata->y = ads.readADC_SingleEnded(0, 0) - joystickoffset_y;
} }
void readSwitches(struct SwitchData *switchdata){ void readSwitches(struct SwitchData *switchdata){
switchdata->magnetSwitch = !digitalRead(13); switchdata->magnetSwitch = !digitalRead(12);
}
void rumble(int duration, int power){
rumbleActivated = true;
analogWrite(15, power);
rumbleDuration = millis() + duration;
} }
void searchBasestation(void){ void searchBasestation(void){
@@ -294,10 +294,10 @@ void calculateWiFiPassword(String WifiSSID, char *WifiPassword)
void sendUdpMessage(struct JoystickData *joystick, struct MPU6050Data *mpu6050data, struct SwitchData *switchdata){ void sendUdpMessage(struct JoystickData *joystick, struct MPU6050Data *mpu6050data, struct SwitchData *switchdata){
Udp.beginPacket(BasestationIp, BasestationPort); Udp.beginPacket(BasestationIp, BasestationPort);
sprintf(sendBuffer, "%06d|%06d|%01d|", joystick->x, joystick->y, joystick->button); sprintf(sendBuffer, "%06d|%06d|%01d|", joystick->x, joystick->y, joystick->button);
sprintf(sendBuffer, "%s%06d|%06d|%06d", sendBuffer, mpu6050data->yaw, mpu6050data->pitch, mpu6050data->roll); sprintf(sendBuffer, "%s%06d|%06d|%06d|", sendBuffer, mpu6050data->yaw, mpu6050data->pitch, mpu6050data->roll);
sprintf(sendBuffer, "%s%01d|%01d", sendBuffer, switchdata->backSwitch, switchdata->magnetSwitch); sprintf(sendBuffer, "%s%01d|%01d", sendBuffer, switchdata->backSwitch, switchdata->magnetSwitch);
// Serial.printf("%s", sendBuffer); Serial.printf("%s", sendBuffer);
// Serial.println(); Serial.println();
Udp.write(sendBuffer); Udp.write(sendBuffer);
Udp.endPacket(); Udp.endPacket();
} }