arduino

Bitirme projem için uzaktan kontrollü denge robotu aldım.Bluetooth ile kontrol edilecek.
biryerde yapılmış buldum fakat yazılımı arduinoya yüklediğimde motorlar bile hareket etmedi..Yardımlarınızı acilen bekliyorum sağolun
http://www.robimek.com/android-kontrollu-dengede-duran-robot-yapimi/
#define version_0.67

//#define DEBUGING
#define BLUE

#include <EEPROMex.h>
#include <I2Cdev.h>
#include <Wire.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include <helper_3dmath.h>
#include <PID_v1.h>
#include <digitalIOPerformance.h>
#define DIGITALIO_NO_INTERRUPT_SAFETY
#define DIGITALIO_NO_MIX_ANALOGWRITE

//Bluetooth Stuff
#include<SoftwareSerial.h>

const int rxpin = 11;
const int txpin = 4;

SoftwareSerial blue(rxpin, txpin);

#define RESTRICT_PITCH

MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
// orientation/motion vars
Quaternion q;
VectorFloat gravity;
float ypr[3];

volatile bool mpuInterrupt = false;
void dmpDataReady() {
mpuInterrupt = true;
}
// Balance PID controller Definitions
#define BALANCE_KP 15
#define BALANCE_KI 90
#define BALANCE_KD 0.8
#define BALANCE_PID_MIN -255
#define BALANCE_PID_MAX 255

#define ROTATION_KP 50
#define ROTATION_KI 300
#define ROTATION_KD 4

#define MOTOR_A_DIR 5
#define MOTOR_A_BRAKE 8
#define MOTOR_B_DIR 6
#define MOTOR_B_BRAKE 12
#define MOTOR_A_PWM 9
#define MOTOR_B_PWM 10

// Motor Misc
#define PWM_MIN 0
#define PWM_MAX 255
float MOTORSLACK_A=32;
float MOTORSLACK_B=39;
#define MOTOR_A_PWM_MAX 255
#define MOTOR_B_PWM_MAX 255

int MotorAspeed, MotorBspeed, MotorSlack,moveState=0,d_speed,d_dir;

double yaw,input,out,setpoint,originalSetpoint,Buffer[3];
double yinput,yout,ysetpoint,yoriginalSetpoint;
//uint32_t timer,timer1;

double bal_kp,bal_ki,bal_kd,rot_kp,rot_ki,rot_kd;
int addressFloat=0;

PID pid(&input,&out,&setpoint,BALANCE_KP,BALANCE_KI,BALANCE_KD,DIRECT);
PID rot(&yinput,&yout,&ysetpoint,ROTATION_KP,ROTATION_KI,ROTATION_KD,DIRECT);

String content = "";
char character;
//String d;

void setup()
{
#ifdef DEBUGING
Serial.begin(9600);
#endif

#ifdef BLUE
blue.begin(9600);
blue.setTimeout(10);
#endif

init_imu();
initmot();

pid.SetMode(AUTOMATIC);
pid.SetOutputLimits(-210, 210);
pid.SetSampleTime(10);
rot.SetMode(AUTOMATIC);
rot.SetOutputLimits(-20, 20);
rot.SetSampleTime(10);

setpoint = 0;
originalSetpoint = setpoint;
ysetpoint = 0;
yoriginalSetpoint = ysetpoint;

addressFloat = EEPROM.getAddress(sizeof(float));
bal_kp=EEPROM.readFloat(addressFloat);
bal_ki=EEPROM.readFloat((addressFloat+=sizeof(float)));
bal_kd=EEPROM.readFloat((addressFloat+=sizeof(float)));
rot_kp=EEPROM.readFloat((addressFloat+=sizeof(float)));
rot_ki=EEPROM.readFloat((addressFloat+=sizeof(float)));
rot_kd=EEPROM.readFloat((addressFloat+=sizeof(float)));

pid.SetTunings(bal_kp,bal_ki,bal_kd);
rot.SetTunings(rot_kp,rot_ki,rot_kd);
}

void loop()
{
getvalues();
new_pid();

#ifdef BLUE
Bt_control();
#endif

#ifdef DEBUGING
printval();
#endif

}

void init_imu()
{
Wire.begin();
mpu.initialize();
devStatus = mpu.dmpInitialize();

mpu.setXGyroOffset(-9);
mpu.setYGyroOffset(-3);
mpu.setZGyroOffset(61);
mpu.setXAccelOffset(-449);
mpu.setYAccelOffset(2580);
mpu.setZAccelOffset(1259);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {

mpu.setDMPEnabled(true);

attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
}

void getvalues()
{

if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

fifoCount = mpu.getFIFOCount();

if ((mpuIntStatus & 0x10) || fifoCount == 1024) {

mpu.resetFIFO();
} else if (mpuIntStatus & 0x02) {

while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

mpu.getFIFOBytes(fifoBuffer, packetSize);

fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

}
yinput = ypr[0]* 180/M_PI;
input = -ypr[1] * 180/M_PI; //change sign if negative
}

void printval()
{
Serial.print(yinput);Serial.print("\t");
Serial.print(yoriginalSetpoint); Serial.print("\t");
Serial.print(ysetpoint); Serial.print("\t");
Serial.print(yout); Serial.print("\t");Serial.print("\t");
Serial.print(input);Serial.print("\t");
Serial.print(originalSetpoint); Serial.print("\t");
Serial.print(setpoint); Serial.print("\t");
Serial.print(out); Serial.print("\t");Serial.print("\t");
Serial.print(MotorAspeed); Serial.print("\t");
Serial.print(MotorBspeed); Serial.println("\t");

}

void Bt_control()
{
if(blue.available())
{
content=blue.readString();
if(content[0]=='F')
setpoint = originalSetpoint - d_speed;//Serial.println(setpoint);}
else if(content[0]=='B')
setpoint = originalSetpoint + d_speed;//Serial.println(setpoint);}
else if(content[0]=='L')
ysetpoint = constrain((ysetpoint + yoriginalSetpoint - d_dir),-180,180);//Serial.println(ysetpoint);}
else if(content[0]=='R')
ysetpoint = constrain(ysetpoint + yoriginalSetpoint + d_dir,-180,180);//Serial.println(ysetpoint);}
else if(content[0]=='S')
d_speed = (content.substring(2)).toInt();//Serial.println(d_speed);}
else if(content[0]=='D')
d_dir = content.substring(2).toInt();//Serial.println(d_dir);}
else if(content[0]=='P')
{
if(content[1]=='S')
{
if(content[2]=='B')
save_pid(1);
else
save_pid(0);
} else if(content[1]=='C')
{
if(content[2]=='B')
{
change_pid(1);
}
else
change_pid(0);
}
}
if(content=="updateb")
return_pid(1);
else if(content=="updater")
return_pid(0);

}
}

void return_pid(bool b)
{
char charVal[10];
String sent = "";
if(b)
{
sent.concat("OP");dtostrf(bal_kp, 5, 3, charVal);sent.concat(charVal);
sent.concat("OI");dtostrf(bal_ki, 5, 3, charVal);sent.concat(charVal);
sent.concat("OD");dtostrf(bal_kd, 5, 3, charVal);sent.concat(charVal);
sent.concat("e");
// Serial.println(sent);
}
else
{
sent.concat("OP");dtostrf(rot_kp, 5, 3, charVal);sent.concat(charVal);
sent.concat("OI");dtostrf(rot_ki, 5, 3, charVal);sent.concat(charVal);
sent.concat("OD");dtostrf(rot_kd, 5, 3, charVal);sent.concat(charVal);
sent.concat("e");Serial.println(sent);
}
blue.print(sent);
}

void change_pid(bool b)
{
blue.print("O");
while(!blue.available());
for(int i=0;i<3;i++) { Buffer=blue.parseFloat(); } if(b) { bal_kp=Buffer[0];bal_ki=Buffer[1],bal_kd=Buffer[2]; } else { rot_kp=Buffer[0];rot_ki=Buffer[1],rot_kd=Buffer[2]; } } void save_pid(bool pid) { addressFloat = 0; if(pid) { EEPROM.updateFloat(addressFloat,bal_kp); EEPROM.updateFloat((addressFloat+=sizeof(float)),bal_ki); EEPROM.updateFloat((addressFloat+=sizeof(float)),bal_kd); } else { EEPROM.updateFloat(addressFloat,rot_kp); EEPROM.updateFloat((addressFloat+=sizeof(float)),rot_ki); EEPROM.updateFloat((addressFloat+=sizeof(float)),rot_kd); } } double compensate_slack(double yOutput,double Output,bool A) { if(A) { if (Output >= 0)
Output = Output + MOTORSLACK_A - yOutput;
if (Output < 0) Output = Output - MOTORSLACK_A - yOutput; } else { if (Output >= 0)
Output = Output + MOTORSLACK_B + yOutput;
if (Output < 0) Output = Output - MOTORSLACK_B + yOutput; } Output = constrain(Output, BALANCE_PID_MIN, BALANCE_PID_MAX); return Output; } void new_pid() { //Compute error pid.Compute(); rot.Compute(); MotorAspeed = compensate_slack(yout,out,1); MotorBspeed = compensate_slack(yout,out,0); motorspeed(MotorAspeed, MotorBspeed); } void initmot() { pinMode(MOTOR_A_DIR, OUTPUT); pinMode(MOTOR_A_BRAKE, OUTPUT); pinMode(MOTOR_B_DIR, OUTPUT); pinMode(MOTOR_B_BRAKE, OUTPUT); analogWrite(MOTOR_A_PWM, 0); analogWrite(MOTOR_B_PWM, 0); } void motorspeed(int MotorAspeed, int MotorBspeed) { if (MotorAspeed >= 0)
{
digitalWrite(MOTOR_A_DIR,HIGH);
digitalWrite(MOTOR_A_BRAKE,LOW);
}
else
{
digitalWrite(MOTOR_A_DIR,LOW);
digitalWrite(MOTOR_A_BRAKE,HIGH);
}

analogWrite(MOTOR_A_PWM,abs(MotorAspeed));

if (MotorBspeed >= 0)
{
digitalWrite(MOTOR_B_DIR,HIGH);
digitalWrite(MOTOR_B_BRAKE,LOW);
}
else
{
digitalWrite(MOTOR_B_DIR,LOW);
digitalWrite(MOTOR_B_BRAKE,HIGH);
}
analogWrite(MOTOR_B_PWM, abs(MotorBspeed));
}