/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
System::init();
//rccEnableCCM(false);
//memset(matrix_heap_buf, 0x55, MATRIX_HEAP_SIZE);
//chHeapObjectInit(&MatrixHeap, matrix_heap_buf, MATRIX_HEAP_SIZE);
//chPoolObjectInit(&matrix_pool, matrix_size, chCoreAllocI);
matrixMempoolStart();
Kalman *kalman = new Kalman();
/*
* Serves timer events.
*/
while (true) {
//BaseThread::sleep(MS2ST(50));
kalman->run();
}
return 0;
}
TEST(KalmanTest, slam1d)
{
Kalman<double, 4, 2> k;
k.X_ << 0.0, 1.0, rnd1(0, 10), rnd1(0, 10); // Позиция, скорость, метка1, метка2
double dt = 0.1;
k.G_(1, 0) = dt; // позиция = позиция + скорость * dt
k.Sigma_(0, 0) = 0.1;
k.Sigma_(1, 1) = 0.1;
k.Sigma_(2, 2) = 100; // Мы не знаем, где метка
k.Sigma_(3, 3) = 100; // Мы не знаем, где метка
k.Q_(0, 0) = 1; //
k.Q_(1, 1) = 1; //
k.Q_(2, 2) = 0; // Метка не двигается
k.H_ << -1.0, 0.0, 1.0, 0.0,
-1.0, 0.0, 0.0, 1.0;
k.R_(0, 0) = 0.1;
k.R_(1, 1) = 0.1;
Eigen::Matrix<double, 2, 1> z;
double mark1 = 10;// rnd(0, 10); // Реальная позиция метки
double mark2 = 20;// rnd(0, 10); // Реальная позиция метки
for (double t = 0; t < 1; t += dt)
{
z(0) = mark1 - k.X_(0);
z(1) = mark2 - k.X_(1);
//std::cout << "X =" << std::endl << k.X_ << std::endl;
//std::cout << "Sigma =" << std::endl << k.Sigma_ << std::endl;
k.predict();
k.correct(z);
}
}
int main(){
printf("---------------------\n");
printf("Starting kalman test.\n");
printf("---------------------\n");
Kalman k;
k.InitPreset();
k.Print();
for (int i = 0; i < 100; ++i)
{
k.Update(0.0, 0.98, 0.0004);
printf("\n");
printf("\n");
printf("%d\n",i);
printf("\n");
k.Print();
}
printf("---------------------\n");
printf("Ending matrix test.\n");
printf("---------------------\n");
return 0;
}
int main( int argc , char **argv )
{
cv::Mat img( cv::Size( 500 , 500 ) , CV_8UC3 );
cv::VideoCapture camera( "http://10.10.10.2:8080/videofeed?frame.mjpeg" );
NoseDetector detector( "nose.xml" );
Kalman kalman;
std::vector< cv::Point > mv;
std::vector< cv::Point > kv;
char key;
while( camera.isOpened() )
{
kalman.setKalman( 0 , 0 );
while( true )
{
camera >> img;
cv::Point predict = kalman.predict();
cv::Point found = detector.findNose( img );
int x = found.x, y = found.y;
kalman.changeMeasure( x , y );
cv::Point measurePoint( x , y );
mv.push_back( measurePoint );
cv::Point statePoint = kalman.estimate();
kv.push_back( statePoint );
cv::circle( img , statePoint , 5 , cv::Scalar( 0 , 255 , 0 ) );
cv::circle( img , measurePoint , 5 , cv::Scalar( 0 , 0 , 255 ) );
for( size_t i = 0 ; i < mv.size() - 1 ; ++i )
cv::line( img , mv[ i ] , mv[ i + 1 ] , cv::Scalar( 0 , 0 , 255 ) );
for( size_t i = 0 ; i < kv.size() - 1 ; ++i )
cv::line( img , kv[ i ] , kv[ i + 1 ] , cv::Scalar( 0 , 255 , 0 ) );
imshow( "Nose Detector" , img );
key = static_cast< char >( cv::waitKey( 100 ) );
if( key > 0 )
break;
}
if( key == 27 )
break;
}
return 0;
}
TEST(KalmanTest, gauss_mul)
{
srand(5);
for (int x = 0; x < 100; x++)
{
double u1 = rnd1(-10, 10); // Матожидания
double u2 = rnd1(-10, 10);
double s1 = rnd1(0.01, 10); // Дисперсии
double s2 = rnd1(0.01, 10);
Kalman<double, 1, 1> k;
EXPECT_DOUBLE_EQ(k.Sigma_(0, 0), 0.0);
k.G_ << 1.0;
k.Q_ << s1; // Шум за один шаг
k.X_ << u1; // Исходная позиция
k.predict();
EXPECT_DOUBLE_EQ(k.X_(0), u1);
EXPECT_DOUBLE_EQ(k.Sigma_(0, 0), s1);
k.H_ << 1.0;
k.R_ << s2; // Дисперсия датчика
Eigen::Matrix<double, 1, 1> z;
z << u2; // Датчик показал позицию
k.correct(z);
EXPECT_NEAR(k.Sigma_(0, 0), s1 * s2 / (s1 + s2), 1E-10); // Произведение нормальных распределений
EXPECT_NEAR(k.X_(0), (u1 * s2 + u2 * s1) / (s1 + s2), 1E-10);
}
}
TEST(KalmanTest, speed) // Сможет ли фильтр Кальмана измерить скорость?
{
double dt = 0.1, v = 1.0; // Квант времени, скорость
Kalman<double, 2, 1> k;
k.X_ << 0.0, 0.0; // Положение, скорость
k.G_(0, 1) = dt; // Задаём движение
k.Q_(0, 0) = 0.1; // Нарастание неопределённости положения
k.Q_(1, 1) = 0.1; // Нарастание неопределённости скорости
k.Sigma_(1, 1) = 10; // Мы не знаем, какая скорость
k.H_ << 1.0, 0.0; // Измеряем положение
k.R_ << 0.1; // Шум измерения
using namespace std;
Eigen::Matrix<double, 1, 1> z;
for (double t = 0; t < 10; t += dt)
{
z(0) = v * t;
/*cout << endl << "t = " << t << endl;
cout << "Z = " << z.transpose() << endl;
cout << "X = " << k.X_.transpose() << endl;
cout << "Sigma =" << endl << k.Sigma_ << endl;*/
k.predict();
k.correct(z);
}
EXPECT_NEAR(k.X_(1), v, 0.0001);
EXPECT_NEAR(k.X_(0), z(0), 0.0001);
}
Vec3 BallManager::calculateRealWorldPosition(MoveBall* ball, Kalman& filter)
{
float x,y,size;
x=ball->position.x;
y=ball->position.y;
size=ball->ballSize;
img->drawCircle(Vec2((int)x,(int)y),(int)(size/2),ColorRgb(255,0,255));
Vec3 position;
position.z=filter.update(2500/size);
//position.z=2500/size;
position.x=(x-((float)img->w/2))*position.z/400;
position.y=(y-((float)img->h/2))*position.z/-400;
return position;
}
void initPidControl()
{
pidInit(&balanceControl.pidPitch);
pidInit(&balanceControl.pidSpeed);
#if 1
//balanceControl.pidPitch.outMax = 64000;
//balanceControl.pidPitch.outMin = -64000;
balanceControl.pidPitch.iMax = 1000;
balanceControl.pidPitch.iMin = -1000;
#endif
balanceControl.pidSpeed.iMax = 1000;
balanceControl.pidSpeed.iMin = -1000;
kalmanPitch.setAngle(imu.euler.pitch);
balanceControl.factorL = 1;
balanceControl.factorR = 1;
balanceControl.spinSpeed = 0;
}
TEST(KalmanTest, base_linearity)
{
constexpr int n = 10, k = 7;
Kalman<double, n, k> kf;
for (int x = 0; x < n; x++)
{
kf.X_(x) = rnd1(-10, 10);
for (int y = 0; y < n; y++)
kf.G_(x, y) = rnd1(-10, 10);
for (int y = 0; y < k; y++)
kf.H_(y, x) = rnd1(-10, 10);
}
Eigen::Matrix<double, n, n> G;
Eigen::Matrix<double, k, n> H;
kf.calcG(G);
kf.calcH(H);
for (int x = 0; x < n; x++)
{
for (int y = 0; y < n; y++)
EXPECT_NEAR(kf.G_(x, y), G(x, y), 1E-10);
for (int y = 0; y < k; y++)
EXPECT_NEAR(kf.H_(y, x), H(y, x), 1E-10);
}
}
void run()
{
Util::CS loggerCS;
Util::Logger logger("timelog.log", Util::Logger::Write, loggerCS);
Kalman kFilter ;
VisionThread vThread(&kFilter);
vThread.start();
// kFilter.Kalman();
#ifdef STR_GUI
Util::CS strCS;
StrategyPacket strPktSh;
StrategyGUIThread strGUIThread = StrategyGUIThread::getInstance(&strPktSh, &strCS);
strGUIThread.start();
TExec tExec(&state);
int prev_tactic_id = -1;
int prev_bot_id = -1;
int prev_play_id = -1;
std::cout << "here4" << std::endl;
//PExec pExec(&state);
#endif
std::cout << "here4" << std::endl;
Tactic::Param paramList[Tactic::MAX_TACTICS];
//tGoalKeeping
TGoalKeepingOurSide tGoalOur0(&state, 0);
TGoalKeepingOurSide tGoalOur1(&state, 1);
TGoalKeepingOurSide tGoalOur2(&state, 2);
TGoalKeepingOurSide tGoalOur3(&state, 3);
TGoalKeepingOurSide tGoalOur4(&state, 4);
Tactic::Param paramGoal;
TGoalie2 tGoalie2(&state, 4);
TShoot tShoot4(&state , 4) ;
Tactic::Param paramShoot;
TDWDefender dwDefend2(&state, 2);
TDWDefender2015 dwDefend20152(&state, 0);
Tactic::Param paramDWDefend;
Tactic::Param paramDWDefend2015;
//tCharge
TCharge tCharge2(&state, 2);
TCharge tCharge1(&state, 1);
Tactic::Param pCharge;
//tReceiveBall
TReceiveBall tReceive3(&state, 3);
TReceiveBall tReceive0(&state, 0);
TReceiveBall tReceive1(&state, 1);
TReceiveBall tReceive2(&state, 2);
Tactic::Param pReceive;
//tCover Bot
TCoverGoal tcover0(&state, 0);
TCoverGoal tcover1(&state,1);
TCoverGoal tcover3(&state,3);
Tactic::Param paramcover;
paramcover.CoverGoalP.distFromGoal = -2*DBOX_WIDTH;
paramList[Tactic::CoverGoal].CoverGoalP.distFromGoal = -2*DBOX_WIDTH;
//CoverGoal2015
TCoverGoal2015 tcover20152(&state,2);
Tactic::Param paramcover2015;
//CoverGoalPair
TCoverGoalPairLeft tcoverleft2(&state,2);
Tactic::Param paramcoverleft;
TCoverGoalPairRight tcoverright0(&state,0);
Tactic::Param paramcoverright;
//tStop Bot
TStop tS0(&state, 0);
TStop tS1(&state, 1);
TStop tS2(&state, 2);
Tactic::Param paramStop;
//tposition
TPosition tPos1(&state, 1);
TPosition tPos2(&state,2);
Tactic::Param pPos;
paramList[Tactic::Position].PositionP.align = false;
paramList[Tactic::Position].PositionP.x= 0;
paramList[Tactic::Position].PositionP.y= 0;
paramList[Tactic::Position].PositionP.finalSlope=PI/2;
//tDefendLine
TDefendLine tDefendLine1(&state,1);
TDefendLine tDefendLine2(&state,2);
Tactic::Param pDefendL1;
paramList[Tactic::DefendLine].DefendLineP.x1 = 0;//BOT_RADIUS/2;
paramList[Tactic::DefendLine].DefendLineP.y1 = -HALF_FIELD_MAXY/2;//-BOT_RADIUS/2;;
paramList[Tactic::DefendLine].DefendLineP.x2 = 0;
paramList[Tactic::DefendLine].DefendLineP.y2 = HALF_FIELD_MAXY/2;//-HALF_FIELD_MAXY;
Tactic::Param pDefendL2;
pDefendL2.DefendLineP.x1 = BOT_RADIUS/2;
pDefendL2.DefendLineP.x2 = HALF_FIELD_MAXX/2;
pDefendL2.DefendLineP.y1 = BOT_RADIUS/2;
pDefendL2.DefendLineP.y2 = HALF_FIELD_MAXY;
//tVelocity
TVelocity tVelocity0(&state,0);
TVelocity tVelocity1(&state,1);
TVelocity tVelocity3(&state,3);
TVelocity tVelocity4(&state,4);
TVelocity tVelocity2(&state,2);
Tactic::Param pVelocity;
pVelocity.VelocityP.vl = 60;
pVelocity.VelocityP.vr = 60;
Tactic::Param pVelocity_1;
pVelocity_1.VelocityP.vl = 30;
pVelocity_1.VelocityP.vr = 30;
Tactic::Param pVelocity_2;
pVelocity_2.VelocityP.vl = 60;
pVelocity_2.VelocityP.vr = 60;
Tactic::Param pVelocity_3;
pVelocity_3.VelocityP.vl = 60;
pVelocity_3.VelocityP.vr = 60;
Tactic::Param pVelocity_4;
pVelocity_4.VelocityP.vl = 80;
pVelocity_4.VelocityP.vr = 80;
paramList[Tactic::Velocity].VelocityP.vl = 60;
paramList[Tactic::Velocity].VelocityP.vr = 60;
//tDefend Point
TDefendPoint tDefendPoint1(&state,1);
Tactic::Param pDefendPoint;
paramList[Tactic::DefendPoint].DefendPointP.x = -HALF_FIELD_MAXX/2;
paramList[Tactic::DefendPoint].DefendPointP.y = 0;
paramList[Tactic::DefendPoint].DefendPointP.radius = HALF_FIELD_MAXX/4;
//tAttack
TAttack tAttack1(&state, 1);
TAttack tAttack0(&state, 0);
TAttack tAttack2(&state, 2);
TAttack tAttack3(&state, 3);
TAttack tAttack4(&state, 4);
TAttackSpline tAttackSpline0(&state , 4);
TAttack2015 tAttack20150(&state , 0) ;
TAttack2015 tAttack20151(&state , 1) ;
TAttack2015 tAttack20152(&state , 2) ;
TAttack2015 tAttack20153(&state , 3) ;
TAttack2015 tAttack20154(&state , 4) ;
TKickoff tKickoff(&state , 4);
TPass tPass(&state , 0);
Tactic::Param pAttack;
paramList[Tactic::Attack].AttackP.rotateOnError = true;
paramList[Tactic::Attack2015].AttackP.rotateOnError = true;
// TestgotoPoint
Strategy::Testgotopoint ttest1(&state,1);
//params1 for SplineGoToPoint
Strategy::SParam params1;
params1.SplineGoToPointP.finalVelocity = 0;
params1.SplineGoToPointP.x = HALF_FIELD_MAXX - GOAL_DEPTH - 6*BOT_RADIUS;
params1.SplineGoToPointP.y = 0;
params1.SplineGoToPointP.finalSlope = 0 ;
params1.SplineGoToPointP.initTraj = 1;
SkillSet sppoint(&state, 4);
//params4 for spline interception with ball
Strategy::SParam params4;
params4.SplineInterceptBallP.vl = 70;
params4.SplineInterceptBallP.vr = 70;
params4.SplineInterceptBallP.initTraj = 1;
SkillSet sball(&state, 4);
// params2 for dwgo to point
Strategy::SParam params2;
SkillSet dwpoint(&state, 0);
SkillSet dwpoint_old(&state, 2);
params2.DWGoToPointP.x = HALF_FIELD_MAXX - GOAL_DEPTH - 6*BOT_RADIUS ;//ForwardX(HALF_FIELD_MAXX);
params2.DWGoToPointP.y = OUR_GOAL_MAXY;
params2.DWGoToPointP.finalSlope = 0;
// params3 for mergeS curve
Strategy::SParam params3 ;
params3.GoToPointP.x = HALF_FIELD_MAXX - GOAL_DEPTH - 4*BOT_RADIUS;
params3.GoToPointP.y = OUR_GOAL_MAXY;//HALF_FIELD_MAXY/2;
params3.GoToPointP.finalslope = 0;
Strategy::SParam params2_old ;
params2_old.GoToPointP.x = HALF_FIELD_MAXX - GOAL_DEPTH - 6*BOT_RADIUS ;
params2_old.GoToPointP.y = OUR_GOAL_MINY ;//HALF_FIELD_MAXY/2;
params2_old.GoToPointP.finalslope = 0;
Strategy::SParam params3_old ;
params3_old.DWGoToPointP.x = HALF_FIELD_MAXX - GOAL_DEPTH - 4*BOT_RADIUS;
params3_old.DWGoToPointP.y = OUR_GOAL_MINY;//HALF_FIELD_MAXY/2;
params3_old.DWGoToPointP.finalSlope = 0;
SkillSet simplegoto(&state, 4);
SkillSet simplegoto_old(&state, 0);
Tactic::Param ptestpoint;
// TestbotRace ttest2(&state,2);
// Tactic::Param ptestrace;
// ptestrace.TestbotRaceP.vl = 100;
// ptestrace.TestbotRaceP.vr = 100;
// FILE *f = fopen("/home/robo/ballPos.txt", "w");
#ifdef BOTLOG
FILE *f = fopen("/home/robo/botplot/compare_dataset/botlog.txt", "w");
fclose(f);
f = fopen("/home/robo/botplot/compare_dataset/response.txt", "w");
fclose(f);
#endif
bool isRunning = true;
// Util::Timer timer;
int loopcount = 0;
unsigned long long int t1=0,t2=0;
usleep(1000);
// ofstream myfile;
// myfile.open ("ballPosLog.txt");
std::cout << "here" << std::endl;
while(running)
{
// timer.start();
state.update();
kFilter.update(state);
//printf("Kalman Updated..\n");
/*unsigned long long int x;
unsigned a, d;
__asm__ volatile("rdtsc" : "=a" (a), "=d" (d));
t2 = ((unsigned long lsadcaong)a) | (((unsigned long long)d) << 32);
printf("\n\tTime taken for while loop\t %f\n",(t2-t1)/3200000.);
t1 = t2;*/ //used to calculate execution time
if(1)
{
// myfile << state.ballPos.x << " -ballPos- " << state.ballPos.y << endl;
#ifdef STR_GUI
{
int test_tactic = 0;
if(test_tactic){
strCS.enter();
//if(strPktSh.which()==1)printf("1\n\n");
if(strPktSh.tactic().tid() != prev_tactic_id){
prev_tactic_id = strPktSh.tactic().tid();
printf("\n\n\n\n\n\n\n");
printf("**************** HELLO ******************");
printf("\n\n\n\n\n\n\n");
}
else{
//printf("**************** BYE BYE ******************");
}
if(strPktSh.tactic().botid() != prev_bot_id){
prev_bot_id = strPktSh.tactic().botid();
}
strCS.leave();
tExec.execute(&state,(Tactic::ID)(prev_tactic_id-1),paramList[prev_tactic_id -1],prev_bot_id);
cout << "\n\n From tester: "<< prev_tactic_id-1 << "\t\t" << prev_bot_id;
}
else{
// Critical Section protected by refBoxCS
strCS.enter();
// Updating from Referee Box and the Game State
// printf("refBoxCmdSh.cmdCounter = %d\n",refBoxCmdSh.cmdCounter);
cout << "\n Play ID " << strPktSh.play() << " " << prev_play_id << "\n";
if (strPktSh.play() != prev_play_id)
{
printf("GOT COMMAND FROM STRATEGY GUI!!!!\n");
prev_play_id = strPktSh.play();
Util::Logger::toStdOut("Command From Refee.. Reselecting play..\n");
// pExec.selectfromGUI(prev_play_id);
}
// pExec.executePlay();
strCS.leave();
}
}
#endif
//printf("Ball Pos: %d %d\n",state.ballPos.x,state.ballPos.y);
// printf(" \n\n %d %d \n\n",state.ourGoalCount,state.oppGoalCount);
// printf("our side %d\n",state.pr_ballOurSide);
//printf("opp side %d\n",state.pr_ballOppSide);
//printf("mid %d\n",state.pr_ballMidField);
//printf("dbox %d\n",state.pr_ball_in_opp_dbox);
//ttest1.execute(ptestpoint);
// tVelocity2.execute(pVelocity);
// tS0.execute(paramStop);
//tAttack2.execute(pAttack);
//tAttack4.execute(pAttack);
// tVelocity1.execute(pVelocity);
//tVelocity0.execute(pVelocity_1);
/* tVelocity2.execute(pVelocity_2);
tVelocity3.execute(pVelocity_3); */
//tVelocity4.execute(pVelocity_4);
// tVelocity0.execute(pVelocity_1);
//tVelocity3.execute(pVelocity);
//tAttackDuo12.execute(pAttack);
// tVelocity0.execute(pVelocity);
//tVelocity2.execute(pVelocity_1);
//tGoalie2.execute(paramGoal);
// tGoalOur2.execute(paramGoal);
//tDefendLine1.execute(pDefendL1);
//tGoalOur2.execute(paramGoal);
// tGoalOur3.execute(paramGoal);
// tAttackSpline0.execute(pAttack) ;
// tPass.execute(pAttack);
if(loopcount++ > 5){
// sppoint.executeSkill(SkillSet::SplineGoToPoint , params1) ;
// params1.SplineGoToPointP.initTraj = 0;
//sball.executeSkill(SkillSet::SplineInterceptBall , params4) ;
//params4.SplineInterceptBallP.initTraj = 0;
//tAttackSpline0.execute(pAttack) ;
// sball.executeSkill(SkillSet::SplineInterceptBall , params4) ;
// params4.SplineInterceptBallP.initTraj = 0;
//tKickoff.execute(pAttack) ;
loopcount = loopcount%1000 + 11;
}
else{
// getchar();
//tVelocity2.execute(pVelocity);
}
//tPass.execute(pAttack);
//tKickoff.execute(pAttack) ;
//tPosition
//tcover3.execute(paramcover);
// tAttack4.execute(paramcover);
// tcover1.execute(paramcover);
//dwDefend2.execute(paramDWDefend);
//dwDefend20152.execute(paramDWDefend2015);
// tcover20152.execute(paramcover2015);
// tGoalOur4.execute(paramGoal);
// tcoverleft2.execute(paramcoverleft);
// tcoverright0.execute(paramcoverright);
// dwpoint.executeSkill(SkillSet::DWGoToPoint,params2) ;
// dwpoint.executeSkill(SkillSet::DWGoToPoint,params2) ;
// dwpoint_old.executeSkill(SkillSet::DWGoToPoint,params3_old) ;
// params3.DWGoToPointP.initTraj = 0;
// params3_old.DWGoToPointP.initTraj = 0;
//sppoint.executeSkill(SkillSet::SplineGoToPoint,params1) ;
//params1.SplineGoToPointP.initTraj = 0;
//dwpoint.executeSkill(SkillSet::DWGoToPoint,params2) ;
// simplegoto.executeSkill(SkillSet::GoToPoint, params3);
//simplegoto_old.executeSkill(SkillSet::GoToPoint, params2_old);
//tAttack4.execute(pAttack);
// tReceive3.execute(pReceive);
//tReceive1.execute(pReceive);
// tAttack3.execute(pAttack);
//tAttack1.execute(pAttack);
//tAttack4.execute(pAttack);
// tcover0.execute(paramcover);
//tcover3.execute(paramcover);
//tCharge1.execute(pCharge);
// tAttack20154.execute(pAttack) ;
//tShoot4.execute(paramShoot) ;
// tAttack0.execute(pAttack);
// tAttack2.execute(pAttack);
// tAttack1.execute(pAttack);
//tAttack3.execute(pAttack);
//tAttack4.execute(pAttack);
//tBackup0.execute(pBackup);
//tDefendArc0.execute(pDefendArc);
// tDefendLine1.execute(pDefendL1);
//tDefendLine2.execute(pDefendL2);
//tAttack2.execute(pAttack);
SkillSet::comm->writeCombinedPacket();
}
else
{
printf("OFF!\n");
tS0.execute(paramStop);
tS1.execute(paramStop);
tS2.execute(paramStop);
}
// printf("tIMER US = %d\n", timer.stopus() );
//moprintf("Ball Pos: %d %d %f\n",state.ballPos.x,state.ballPos.y,state.homeAngle[2]);
usleep(16000); // Adding sleep to this thread of execution to prevent CPU hogging
}
vThread.stop();
Util::Logger::toStdOut("Exiting process");
// myfile.close();
return;
}
int main() {
// MPU6050 object "MPU6050.h"
MPU6050 accelgyro;
// Kalman filter Object "Kalman.h"
Kalman kalman;
// Xbee serial baudrate
xBee.baud(38400);
pc.baud(9600);
// Initialize timers
Timer timer, code, encTimer;
// Start timers
timer.start();
code.start();
encTimer.start();
// Reset timer values
timer.reset();
code.reset();
encTimer.reset();
//Encoder 1 interrupts
enc1a.rise(&incrementEnc1a);
enc1b.rise(&incrementEnc1b);
enc1a.fall(&setEnc1aFall);
enc1b.fall(&setEnc1bFall);
enc2a.rise(&incrementEnc2a);
enc2b.rise(&incrementEnc2b);
enc2a.fall(&setEnc2aFall);
enc2b.fall(&setEnc2bFall);
// Debug leds
myled1 = 0;
myled2 = 0;
myled3 = 0;
// Encoder 1 initializations
newTime =0; oldTime = 0; lastEncTime = 0;
enc1Speed = 0; enc2Speed = 0;
// MPU6050 initializations
accelgyro.initialize();
accelgyro.setFullScaleGyroRange(0);
accelgyro.setFullScaleAccelRange(0);
// Initialize MotorShield object
shield.init();
float measuredTheta , measuredRate, newTheta,calcRate;
float position= 0, velocity = 0, lastPosition = 0, lastVelocity =0;
float angleOffset = 0, positionOffset = 0,lastAngleoffset = 0;
// Position control constants, if necessary
float zoneA = 16000.0f, zoneB = 8000.0f, zoneC = 2000.0f;
float multiplier = 1.0f;
float zoneAscale = 600*2*multiplier,
zoneBscale = 800*2*multiplier,
zoneCscale = 1000*2*multiplier,
zoneDscale = 1500*2*multiplier,
velocityScale = 60*2*multiplier;
// Serial communication buffer
char buffer[40];
int i, strSize;
// Control parameters
float k0,k1,k2,k3,tref;
float x, dotx, deltaX, deltaT, lastX = 0;
// Helper variables
float waittime , u, diff, dt;
float error = 0,lastError = 0;
int counter = 0;
float pTerm = 0,dTerm = 0,iTerm = 0;
while(1) {
///////////////////////////////////////////
// Read serial data and update constants //
///////////////////////////////////////////
i = 0;
char a;
while(pc.readable()){
a = pc.getc();
buffer[i++] = a;
printf("%c\n", a );
myled1 = 1;
wait(0.001);
}
strSize = i;
string receive(buffer,strSize); // Changes from char to string
if(strSize > 0){
printf("%s\n", receive);
assignGainsFromString(&k[0], &k[1], &k[2], &k[3], &k[4],receive);
}
// Below is an attempt to control the robot position,
// by dividing it into "zones" and changing the angle accordingly.
//
/////////////////////////////
// Generate encoder offset //
/////////////////////////////
// position = (float)enc2total;
// positionOffset = position;
// //if((encTimer.read_us() - lastEncTime) > 0.0f ) { // every 100 ms
// float sign = 0;
// if(positionOffset > 0) sign= 1;
// else sign = -1;
// if(abs(positionOffset) > zoneA) angleOffset += 1.0f/zoneAscale*positionOffset;
// else if(abs(positionOffset) > zoneB) angleOffset += 1.0f/zoneBscale*positionOffset;
// else if(abs(positionOffset) > zoneC) angleOffset += 1.0f/zoneCscale*positionOffset;
// else angleOffset += positionOffset/zoneDscale;
// printf("angleOffset: %f, positionoffset: %f\n", angleOffset, positionOffset );
// // Estimate velocity
// //
// velocity = (position - lastPosition);
// lastPosition = position;
// angleOffset += velocity/velocityScale;
// angleOffset = constrain(angleOffset,-10, 10);
// lastAngleoffset = angleOffset;
// //}
// angleOffset = constrain(angleOffset,lastAngleoffset - 1, lastAngleoffset + 1);
timer.reset();
///////////////////////////
// Get gyro/accel values //
///////////////////////////
accelgyro.getAcceleration(&ax,&ay,&az);
measuredRate = accelgyro.getRotationX()*1.0/131.0f; // Units depend on config. Right now 250o/s
measuredTheta = -atan(1.0*ay/az);
measuredTheta = measuredTheta*180/PI; // measured in degrees
///////////////////
// Kalman Filter //
///////////////////
dt = (double)(code.read_us() - oldTime)/1000000.0f;
newTheta = kalman.getAngle(measuredTheta,
-measuredRate, dt);
//DEBUG: printf("%g \n", (double)(code.read_us() - oldTime)/1000000.0f);
oldTime = code.read_us();
//////////////////
// Control loop //
//////////////////
// Set control variable to zero
u = 0;
// Extract constants from k vector, which has the serial readings.
float kp = k[0];
float ki = k[1];
float kd = k[2];
tref = k[3] - angleOffset;
waittime = k[4];
if(newTheta >= 50 || newTheta <= -50){
u = 0;
}else{
// Define error term
error = newTheta - tref;
// Proportional term
pTerm = kp*error;
//Integral term
iTerm += ki*error*dt*100.0f;
// Prevent integration windup:
if(iTerm >= 100) iTerm = 100;
else if (iTerm <= -100) iTerm = -100;
u = -(iTerm + pTerm);
// Calculated derivative based on smoothened angle.
// There are two other sources for the angle here: kalman.getRate(); and measuredRate.
calcRate = -(error-lastError)/dt;
// Derivative term
if(kd != 0)
dTerm = kd*calcRate/100.0f;
lastError = error;
u += dTerm;
// Correct for dead zone --- Did not have successful results but I'll leave it here.
// int deadzone = 20;
// if(u < -deadzone) u = u + deadzone;
// else if(u > deadzone) u = u - deadzone;
// else u = 0;
// // Include saturation
u = constrain(u,-400,400);
// Flash LED to indicate loop
if(counter % 50 == 0){
myled3 = !myled3;
counter = 0;
}
counter++;
}
lastError = error; // update angle
if(counter % 50 == 0){
myled3 = !myled3;
// xBee.printf("%f,%f\n", newTheta,u);
counter = 0;
}
// Set motor speed
shield.setM1Speed(-(int)u);
shield.setM2Speed((int)u);
// DEBUG over serial port. Use with MATLAB program "serialPlot.m" to plot these results.
printf("%f,%f,%f\n", measuredTheta, newTheta , u);
// Hold the timer until desired sampling time is reached.
while(timer.read_us() < waittime*1000);
// Turn off serial transmission LED
myled1 = 0;
}
}
// [[Rcpp::export]]
mat KalmanCpp(mat Z) {
Kalman K;
mat Y = K.estimate(Z);
return Y;
}
void pidControl()
{
angleFiltered = kalmanPitch.getAngle(imu.euler.pitch, imu.gyro.y/131.0, (double)1/FREQ);
gyroFiltered = LPF(gyroFiltered, imu.gyro.y);
//kalman_filter(imu.euler.pitch, imu.gyro.y/131.0, &angleFiltered, &gyroFiltered);
#if 0
balanceControl.pidPitch.desired = pidUpdate(&balanceControl.pidSpeed, balanceControl.PwmLeft) + angleOffset;
balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, angleFiltered);
currendSpeed = (currendSpeed + balanceControl.PwmLeft * 0.004) * 0.999;
balanceControl.PwmLeft += currendSpeed;
#endif
#if 0
balanceControl.pidPitch.desired = pidUpdate(&balanceControl.pidSpeed, balanceControl.PwmLeft) + angleOffset;
balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, angleFiltered);
#endif
#if 0
//Kp:5 Kd:1 offset:-3 20/10/-6 0.5/0.2/06
balanceControl.PwmLeft = -(1000*(balanceControl.pidPitch.Kp * ((angleFiltered + angleOffset) /90)) + balanceControl.Kd * imu.gyro.y);
#endif
#if 0
currendSpeed *= 0.7;
currendSpeed = currendSpeed + balanceControl.PwmLeft * 0.3;
position += currendSpeed;
position -= speed_need;
if(position<-6000000) position = -6000000;
if(position> 6000000) position = 6000000;
balanceControl.PwmLeft = balanceControl.pidPitch.Kp * (angleOffset - angleFiltered)
+balanceControl.Kd * gyroFiltered
-balanceControl.pidSpeed.Ki * position
-balanceControl.pidSpeed.Kd * currendSpeed;
balanceControl.PwmLeft = -balanceControl.PwmLeft;
if(balanceControl.PwmLeft<-60000) balanceControl.PwmLeft = -60000;
if(balanceControl.PwmLeft> 60000) balanceControl.PwmLeft = 60000;
if(balanceControl.PwmLeft > - 100 && balanceControl.PwmLeft < 100) { balanceControl.PwmLeft = 0; }
balanceControl.PwmRight = balanceControl.PwmLeft;
#endif
#if 0
float gap = abs(balanceControl.pidPitch.desired - imu.euler.pitch);
if(gap > 150)
{
if(flag == 0)
{
flag = 1;
balanceControl.pidPitch.Kp *= 10;
balanceControl.pidPitch.Ki *= 10;
balanceControl.pidPitch.Kd *= 10;
//balanceControl.Kd *= 2;
}
}
else
{
if(flag == 1)
{
flag = 0;
balanceControl.pidPitch.Kp /= 10;
balanceControl.pidPitch.Ki /= 10;
balanceControl.pidPitch.Kd /= 10;
//balanceControl.Kd /= 2;
}
}
#endif
#if 0 //works
balanceControl.speed = balanceControl.speed * 0.05 + pidUpdate(&balanceControl.pidSpeed, balanceControl.speed) * 0.95;
gyroFiltered = 0.05 * imu.gyro.y + gyroFiltered * 0.95;
//angleFiltered = 0.2 * imu.euler.pitch + angleFiltered * 0.8;
//angleFiltered = kalmanPitch.getAngle(imu.euler.pitch, imu.gyro.y/131.0, (double)1/250);
//angleFiltered = kalman(imu.euler.pitch, imu.gyro.y, (double)1/FREQ);
balanceControl.pidPitch.desired = balanceControl.speed + angleOffset;
balanceControl.speed = pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd;
//balanceControl.speed += pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd;
//balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, imu.euler.pitch);
if(balanceControl.speed < 0.100 && balanceControl.speed > -0.100) { balanceControl.speed = 0; } //dead-band of PWM
#endif
#if 1
balanceControl.speed = LPF((float)(balanceControl.PwmLeft + balanceControl.PwmRight)/5600, balanceControl.speed);
balanceControl.speed = pidUpdate(&balanceControl.pidSpeed, balanceControl.speed);
balanceControl.pidPitch.desired = balanceControl.speed + angleOffset;
balanceControl.speed = pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd;
if(balanceControl.speed < 0.050 && balanceControl.speed > -0.050) { balanceControl.speed = 0; } //dead-band of PWM
#endif
//printf("dev:%6.4f ", balanceControl.pidPitch.derivative);
printf("AngleSet:%4.2f AngleRef:%4.2f Angle:%4.2f error:%6.4f ", angleOffset, balanceControl.pidPitch.desired, angleFiltered, balanceControl.pidPitch.error);
printf("\t| Kp:%3.2f Kd:%3.3f sumerror:%6.2f", balanceControl.pidPitch.Kp, balanceControl.Kd, balanceControl.pidPitch.sumError);
printf("\t| Kp:%3.2f Ki:%3.3f sumerror:%6.2f", balanceControl.pidSpeed.Kp, balanceControl.pidSpeed.Ki, balanceControl.pidSpeed.sumError);
printf("\t| speedref:%6.2f speed%6.2f error:%6.2f\r\n", balanceControl.pidSpeed.desired, balanceControl.speed, balanceControl.pidSpeed.error);
/*
printf("\t| Kp:%4.2f Ki:%4.2f Kd:%4.2f Speed:%4.2f angle:%4.2f |\t error:%6.4f sumerror:%6.4f Iterm:%6.4f | PWM:%6.3f\r\n",
balanceControl.pidSpeed.Kp, balanceControl.pidSpeed.Ki, balanceControl.pidSpeed.Kd,
balanceControl.pidSpeed.desired, balanceControl.pidPitch.desired,
balanceControl.pidSpeed.error, balanceControl.pidSpeed.sumError, balanceControl.pidSpeed.intergal,
balanceControl.speed);
*/
balanceControl.PwmLeft = 2800 * (balanceControl.speed * balanceControl.factorL - balanceControl.spinSpeed);
balanceControl.PwmRight = 2800 * (balanceControl.speed * balanceControl.factorR + balanceControl.spinSpeed);
}
void run()
{
//printf("WORKERRRR\n");
//assert(false);
Util::CS loggerCS;
Util::Logger logger("timelog.log", Util::Logger::Write, loggerCS);
Kalman kFilter;
VisionThread vThread(&kFilter);
vThread.start();
PExec pExec(&state);
Util::Logger::toStdOut("Waiting for signal from Referee Box and SSLVision Client\n");
unsigned char refBoxCmdCounter = -1;
HAL::RefBoxCmd refBoxCmdSh;// = new RefBoxCmd();
#ifdef RUN_REFBOX
Util::CS refBoxCS;// = new CS();
RefBoxThread refBoxThread = RefBoxThread::getInstance(&refBoxCmdSh, &refBoxCS);
refBoxThread.start();
#endif // RUN_REFBOX
state.refreeUpdated = false;
usleep(1000);
while (running)
{
writer_preference->enter();
writer_mutex->enter();
state.update();
kFilter.update(state);
writer_mutex->leave();
writer_preference->leave();
#ifdef RUN_REFBOX
{
// Critical Section protected by refBoxCS
refBoxCS.enter();
// Updating from Referee Box and the Game State
// printf("refBoxCmdSh.cmdCounter = %d\n",refBoxCmdSh.cmdCounter);
if (refBoxCmdCounter != refBoxCmdSh.cmdCounter)
{
printf("GOT COMMAND FROM REFEREE!!!!\n");
state.refreeUpdated = true;
Util::Logger::toStdOut("Ref Box Updated to %d\n", refBoxCmdSh.cmdCounter);
refBoxCmdCounter = refBoxCmdSh.cmdCounter;
state.updateStateFromRefree(refBoxCmdSh);
}
printf("%d\t%d\n",refBoxCmdSh.goalsBlue,refBoxCmdSh.goalsYellow);
refBoxCS.leave();
} // End of Critical Section protected by refBoxCS
#endif // RUN_REFBOX
#ifdef USE_FAKE_REFREE
fake_refree(refBoxCmdSh);
if (refBoxCmdCounter != refBoxCmdSh.cmdCounter)
{
state.refreeUpdated = true;
refBoxCmdCounter = refBoxCmdSh.cmdCounter;
state.updateStateFromRefree(refBoxCmdSh);
}
#endif
{
if(state.refreeUpdated)
{
state.refreeUpdated = false;
Util::Logger::toStdOut("Command From Refee.. Reselecting play..\n");
pExec.selectPlay();
}
else if (pExec.playTerminated())
{
Util::Logger::toStdOut("*************Play terminated.Select new play\n*********************");
pExec.evaluatePlay();
pExec.selectPlay();
}
printf("executing play!\n");
pExec.executePlay();
}
SkillSet::comm->writeCombinedPacket();
// printf("%d\t%d\n",state.ourGoalCount,state.oppGoalCount);
usleep(16000); // Adding sleep to this thread of execution to prevent CPU hogging
}
vThread.stop();
Util::Logger::toStdOut("Exiting process");
}
int main(int argc, char **argv) {
/* INS ins(Eigen::Quaterniond(0, 1, 0, 0),
Eigen::Vector3d(1, 2, 3),
Eigen::Vector3d(4, 5, 6),
Eigen::Vector3d(0, 0, -9.8));
INS::Measurement measurement_prev = {
Eigen::Vector3d(.1, .1, .1),
Eigen::Vector3d(-.2, -.2, -.2)
};
INS::Measurement measurement = {
Eigen::Vector3d(.11, .11, .11),
Eigen::Vector3d(-.21, -.21, -.21)
};
ins.update(measurement_prev, .01);
ins.update(measurement, .01);
const INS::State &state = ins.getState();
std::cout << state.p_nav << std::endl;*/
/* Kalman kalman;
kalman.x = Eigen::Matrix<double, 15, 1>::Ones();
kalman.P = Eigen::Matrix<double, 15, 15>::Ones() + 10*Eigen::Matrix<double, 15, 15>::Identity();
INS::State state;
state.q_imu2nav = Eigen::Quaterniond(1, 2, 3, 4).normalized();
state.w_imu = Eigen::Vector3d(1, 2, 3);
state.a_imu = Eigen::Vector3d(-1, -3, 5);
state.g_nav = Eigen::Vector3d(0, 0, -9.8);
Kalman::PredictConfig predict;
predict.R_g = Eigen::DiagonalMatrix<double, 3, 3>(
4.6254e-6, 4.6254e-6, 4.6254e-6);
predict.R_a = Eigen::DiagonalMatrix<double, 3, 3>(
1.1413e-6, 1.1413e-6, 1.1413e-6);
predict.Q_b_g = Eigen::DiagonalMatrix<double, 3, 3>(
1.2217e-8, 1.2217e-8, 1.2217e-8);
predict.Q_b_a = Eigen::DiagonalMatrix<double, 3, 3>(
1.9700e-7, 1.9700e-7, 1.9700e-7);
kalman.predict(state, predict, .01);
std::cout << kalman.x << std::endl;
std::cout << kalman.P << std::endl;*/
Kalman kalman;
kalman.x = Eigen::Matrix<double, 15, 1>::Ones();
kalman.P = 1e-6*Eigen::Matrix<double, 15, 15>::Ones() + 10*Eigen::Matrix<double, 15, 15>::Identity();
INS::State state;
state.q_imu2nav = Eigen::Quaterniond(1, 2, 3, 4).normalized();
state.w_imu = Eigen::Vector3d(1, 2, 3);
state.a_imu = Eigen::Vector3d(-1, -3, 5);
state.g_nav = Eigen::Vector3d(0, 0, -9.8);
state.v_nav = Eigen::Vector3d(2, 0, 0);
Kalman::UpdateConfig update;
update.R_g = Eigen::DiagonalMatrix<double, 3, 3>(
4.6254e-6, 4.6254e-6, 4.6254e-6);
update.R_a = Eigen::DiagonalMatrix<double, 3, 3>(
1.1413e-04, 1.1413e-04, 1.1413e-04);
update.R_m = Eigen::DiagonalMatrix<double, 3, 3>(
1e-6, 1e-6, 1e-6);
update.R_d.fill(0);
for (int i=0; i<4; i++)
update.R_d(i, i) = 4.9e-5;
update.R_z = 3e-3;
update.beam_mat <<
0.5, 0, 0.86603,
-0.5, 0, 0.86603,
0, -0.5, 0.86603,
0, 0.5, 0.86603;
update.r_imu2dvl <<
0, 0, -0.102-0.076;
update.q_imu2dvl = Eigen::Quaterniond(
0.000284, 0.394308, -0.918965, -0.005013);
update.r_imu2depth <<
0.445 - 0.431, 0.102, -0.051 - 0.076;
update.m_nav <<
-2244.2, 24151.0, -40572.8;
Eigen::Matrix<double, 11, 1> z;
z.fill(2);
kalman.update(z, true, true, {{true, true, true, true}}, true, state, update);
// std::cout << kalman.x << std::endl;
// std::cout << kalman.P << std::endl;
}
