Final Project

Software

The software development was quite the same as the electricity one. I started with small systems, make them do what I want and put the subsystems together. Find the finished code below. As you will see I implemented not only the three degrees of freedom I needed but also the other six. The reason is to prepare also the software to get developed by others. So maybe someone else want to track another movement to control another machine or tool. See the code below. There I commented the single parts.

//sensor readout
#include <Wire.h>

long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;

long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;

int minVal=265;
int maxVal=402;

double tiltX;
double tiltY;
double tiltZ;

//grab control (to acivate the vacuum pump)
int relais = 6;
int grab = 5;
int grabstatus = 0;


//servos ( to make the robot arm move)
#include <Servo.h>
Servo servo1;  //add the servos
Servo servo2;
Servo servo3;

int position1 = 90;   //variables to controll the servos
int leftspeedZ = 90;
int rightspeedZ = 90;
int position2 = 90;
int position3 = 90;

int servopin1 = 8;   //pins where the servos are attached
int servopin2 = 9;
int servopin3 = 10;



//movedelay (stops all robot motions)
int movedelay = 4;
int movedelaystatus = 0;




void setup() {
  Serial.begin(9600);
  Wire.begin();
  setupMPU();            //starts the connection to the mpu sensor
  servo1.attach(8);
  servo2.attach(9);
  servo3.attach(10);
pinMode(relais, OUTPUT);
pinMode(grab, INPUT);
pinMode(movedelay, INPUT);
}


void loop()
{
recordAccelRegisters();
recordGyroRegisters();
movedelaystatus=digitalRead(movedelay);

if(movedelaystatus==LOW)   //test if the movedelay-button is pressed
{
grabcontrol();
servocontrol();
}
else{                      //stop all servos
  servo1.write(90);
  servo2.write(90);
  servo3.write(90);
}
printData();                //show the sensordata


}

void setupMPU(){
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU
  Wire.write(0x6B); //Accessing the register 6B -
  Wire.write(0b00000000); //Setting SLEEP register to 0.
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1B); //Accessing the register 1B
  Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1C); //Accessing the register 1C
  Wire.write(0b00000000); //Setting the accel to +/- 2g
  Wire.endTransmission();
}

void recordAccelRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processAccelData();
  processAngleData();
}

void processAccelData(){
  gForceX = accelX / 16384.0;            //calculate the gForce data
  gForceY = accelY / 16384.0;
  gForceZ = accelZ / 16384.0;

}

void processAngleData()
{
int xAng = map(accelX,minVal,maxVal,-90,90);
int yAng = map(accelY,minVal,maxVal,-90,90);
int zAng = map(accelZ,minVal,maxVal,-90,90);
tiltX= RAD_TO_DEG * (atan2(-yAng+180, -zAng+180)+PI);        //calculate the tilt angle
tiltY= RAD_TO_DEG * (atan2(-xAng+180, -zAng+180)+PI);
tiltZ= RAD_TO_DEG * (atan2(-yAng+180, -xAng+180)+PI);
}

void recordGyroRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into gyroX
  gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into gyroY
  gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into gyroZ
  processGyroData();
}

void processGyroData() {
  rotX = gyroX / 131.0;        //calculate the rotation accelleration
  rotY = gyroY / 131.0;
  rotZ = gyroZ / 131.0;
}

void printData() {

  Serial.print(" X=");                    //write all data into the serial monitor
  Serial.print(rotX);
  Serial.print(" Y=");
  Serial.print(rotY);
  Serial.print(" Z=");
  Serial.print(rotZ);
  Serial.print("         gForceX=");
  Serial.print(gForceX);
  Serial.print(" gForceY=");
  Serial.print(gForceY);
  Serial.print(" gForceZ=");
  Serial.print(gForceZ);
  Serial.print("         tiltX=");
  Serial.print(tiltX);
  Serial.print(" tiltY=");
  Serial.print(tiltY);
  Serial.print(" tiltZ=");
  Serial.println(tiltZ);
 }

 void servocontrol()                     //controll the servos with the sensor data
{
  //Servo1: track the rotation acceleration and rotate the servos
    while(rotZ>10)
    {
    leftspeedZ = map(rotZ,15,200,90,120);
    servo1.write(leftspeedZ);
    recordGyroRegisters();
    }

    while(rotZ<-10)
   {
    rightspeedZ = map(rotZ,-15,-200,90,60);
    servo1.write(rightspeedZ);
    recordGyroRegisters();
    }


//Servo2: track the tilt around the Y-axis and rotate the second servo
position2 = map(tiltY,135,225,100,80);
 if((position2<84) && (position2>96))
  {
    servo2.write(90);
  }
else
  {
    servo2.write(position2);
  }

  //Servo3: track the tilt around the X-axis and rotate the third servo
  position3 = map(tiltX,135,225,100,0);
  if((position3<84) && (position3>96))
  {
    servo3.write(90);
  }
else
  {

    servo3.write(position3);
  }
}

 void grabcontrol()         //track the status of the grab-button and start the pump
{
  grabstatus=digitalRead(grab);
if (grabstatus == LOW)
{
digitalWrite(relais, LOW);
}
else
{
digitalWrite(relais, HIGH);
}
}

As you see the code includes some variables which could be edited for various cases. So for example the speed can changed by editing the map-function or the sensitivity can ne set up by changing the if clauses.
First Tests

Downloads

final Software

download