2018-02-18 14:02:33 +05:30

284 lines
6.7 KiB
C++

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#define LTMDIR 7
#define RTMDIR 4
#define LTMPWM 6
#define RTMPWM 5
#define LTMINT 2
#define RTMINT 3
#define MPUINT 8
MPU6050 mpu;
int rn = 0, ln = 0;
float r1 = 0, l1 = 0;
float rv = 0, lv = 0;
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
bool rDir = 0;
bool lDir = 0;
uint32_t t, t1, t2;
int turn = 0;
int lout = 0;
int rout = 0;
double cgcerr = 0;
volatile uint8_t rc = 0;
volatile uint8_t lc = 0;
volatile bool mpuInterrupt = false;
Quaternion q;
VectorFloat gravity;
int16_t gyro[3];
float gy;
float ypr[3];
int m = 30;
float cg = 0;
float cgoffset = 0;
float angs = 20.0;
float alpha = 0, kcg, kcgd, kcgi, kangp = 1.0, kangd = 10.0;
float angle = 0;float pangle = 0;
void motorControl();
void getYPR();
void calculate();
void setup() {
Wire.begin();
Serial.begin(57600);
delay(10);
mpuInit();
//delay(15000);
intInit();
delay(10);
getYPR();
cgoffset = 3.2;
pinMode(LTMINT, INPUT);
pinMode(RTMINT, INPUT);
pinMode(LTMDIR, OUTPUT);
pinMode(RTMDIR, OUTPUT);
pinMode(LTMPWM, OUTPUT);
pinMode(RTMPWM, OUTPUT);
analogWrite(RTMPWM,1);
analogWrite(LTMPWM,1);
t=millis();
t1=millis();
//motorControl( 100,100);
}
int out = 0;
void loop() {
getYPR();
t2 = millis() - t;
odor();
cgPID(t2);
angle = cg - ypr[1];
if(abs(pangle-angle)<4){
out = anglePD() + t2 * alpha / 100.0;
pangle = angle;
}
rc = lc = 0;
lout = out + turn;
rout = out - turn;
rDir = ( (rout > 0) ? 1 : 0);
lDir = ( (lout > 0) ? 1 : 0);
lout = abs( lout );
rout = abs( rout );
if(lout != 0)
{
if( (lout > 0) && (lout < m) )lout = m;
}
if(rout != 0)
{
if( (rout > 0) && (rout < m) )rout = m;
}
lout = constrain(lout, -200, 200);
rout = constrain(rout, -200, 200);
if(abs(angle) > 40)
motorControl( 0, 0);
else
motorControl( rout, lout);
Serial.print(out);
Serial.print("\t");
//Serial.print(", ");
Serial.print(ypr[1]);
Serial.print("\t");
Serial.println(abs(angle));
//Serial.print("\tra la\t");
//Serial.print(ra);
//Serial.print(",");
//Serial.println(la);
if(Serial.available()>2){
switch (Serial.read())
{
case 'p':kangp = Serial.parseFloat();break;
case 'i':kcgi = Serial.parseFloat();break;
case 'a':kcg = Serial.parseFloat();break;
case 'b':kcgd = Serial.parseFloat();break;
case 'd':kangd = Serial.parseFloat();break;
case 's':cg = Serial.parseFloat();t = millis();break;
case 't':angs = Serial.parseFloat();break;
//case 'a':an=Serial.parseFloat();break;
case 'm':m = Serial.parseFloat();break;
}
Serial.read();
}
//Serial.print(asp);Serial.print("\t");Serial.print(ap);Serial.print("\t");Serial.print(li);Serial.print("\t");Serial.print(ad);Serial.print("\t");Serial.println(lp);
}
void cgPID(uint32_t t){
float err = t * alpha / 100.0 - (rv + lv) / 2.0;
cgcerr += kcgi * err;
cgcerr = constrain( cgcerr, -5, 5);
cg = err * kcg + cgcerr + kcgd * gy + cgoffset;
cg = constrain(cg, -10, 10);
}
float anglePD()
{
float err = alpha - angle;
//return (err/abs(err))*(pow(kangp,abs(err)/57.2958)-1)*255.0/(pow(kangp,angs/57.2958)-1) + (gy) * kangd;
return (err * abs(angle) * kangp + gy * kangd);
}
void odor(){
t1 = (millis() - t1);
if(!lDir)
{
lv = -1 * lc;
lc = 0;
}
else
{
lv = lc;
lc = 0;
}
if(!rDir)
{
rv = -1 * rc;
rc = 0;
}
else
{
rv = rc;
rc=0;
}
r1 += rv;
l1 += lv;
rv = rv * 1000.0 / t1;
lv = lv * 1000.0 / t1;
t1=millis();
if(abs(r1) > 29)
{
if(r1 > 29)
{
rn++;
r1 -= 30;
}
else
{
rn--;
r1 += 30;
}
}
if(abs(l1) > 29)
{
if(l1 > 29)
{
ln++;
l1 -= 30;
}
else
{
ln--;
l1 += 30;
}
}
}
void motorControl(int a, int b)
{
if(lDir)
PORTD |= _BV(RTMDIR);
else
PORTD &= ~_BV(RTMDIR);
if(rDir)
PORTD |= _BV(LTMDIR);
else
PORTD &= ~_BV(LTMDIR);
OCR0B = ( ( abs( a ) <= 200 ) ? a : 0 );
OCR0A = ( ( abs( b ) <= 200 ) ? b : 0 ) * 1.2 + 10;
}
void getYPR(){
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);
mpu.dmpGetGyro(gyro, fifoBuffer);
ypr[1] *= 180 / PI;
gy = -gyro[1]/100.0;
}
}
void intInit(){
mpu.resetFIFO();
TCCR1A=0;
TCCR1B=0;
//TCNT1=64886;
TCCR1B |= (1 << CS12);
TIMSK1 |= (1 << TOIE1);
attachInterrupt(digitalPinToInterrupt(RTMINT),RME,RISING);
attachInterrupt(digitalPinToInterrupt(LTMINT),LME,RISING);
}
void LME(){
lc++;
}
void RME(){
rc++;
}
ISR (PCINT0_vect){
if(digitalRead(8) == 1)mpuInterrupt = 1;
}
void mpuInit(){
mpu.initialize();
pinMode(MPUINT, INPUT);
mpu.testConnection();
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(-1);
mpu.setYGyroOffset(-1);
mpu.setZGyroOffset(255);
mpu.setXAccelOffset(705);
mpu.setYAccelOffset(-139);
mpu.setZAccelOffset(1219);
mpu.setDMPEnabled(true);
if (devStatus == 0) {
mpu.setDMPEnabled(true);
*digitalPinToPCMSK(MPUINT) |= bit (digitalPinToPCMSKbit(MPUINT));
PCIFR |= bit (digitalPinToPCICRbit(MPUINT));
PCICR |= bit (digitalPinToPCICRbit(MPUINT));
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
}