Inertial Sensor probs

My teams programmer is having some problems with coding our inertial sensor. Does anybody have a sample code that we could look at? If you do that would be great. Thanks!

Are you IQ or V5? I have some sample V5 code from last season’s PID control for inertial sensors

1 Like

Some more information would be helpful.

What sort of problems are they having? What have you tried to troubleshoot/isolate the issue?

There are numerous topics in the forums here as well as knowledge base articles that would be good starting points. With more information and ideas on what you’ve tried, folks are more able to help.

1 Like

We are in V5. If you have a sample code that would be great.

Would you like the sample of the entire PID code or just the inertial sensor part?

1 Like

just the inertial would be fine

1 Like
#include "cmath"  
#include "vex.h"  
#include "motorControl.h"  
#include <ctime>
timer T1;    
timer T2;
timer T3;
timer TACC;
int act=0;
double now_angle=0;

void stop(brakeType type)
{
  L1.stop(type);
  L2.stop(type);
  R1.stop(type);
  R2.stop(type);
}


int sgn(double output)
{
    if(output<0)
        return -1;//=*-1
    else
        return 1;
}

double return_angle()      
{
  return tuoluoyi.angle();
}

void just_right_spin(int output)  
{
  L1.spin(forward,output,pct);
  L2.spin(forward,output,pct);
  R1.spin(reverse,output,pct);
  R2.spin(reverse,output,pct);
}


void pidturn(double target_degree,int max_speed)

{       
    //inertial_clear();

    double now_angle=return_angle();
    double Kp=0.8;//0.775
    double Ki=0.001;//0.001
    double Kd=0;//3*/
    double err_now=0;
    double err_last=0;
    //double prim_angle=return_angle(dec_flag);
    //double value_now=0;
    //double value_last=0;
    double EI=0,ED=0;
    double output;   
    int sampletime=10;
    T2.clear();

    now_angle=return_angle();
    //value_now=0;       
    
    err_now=target_degree-now_angle; 
    /*if(fabs(err_now)<100)    
    {
      Kp=0.9;//0.775
      Ki=0.01;//0.001
      Kd=3;//3
    }
    */
    Brain.Screen.clearScreen();
    Brain.Screen.drawRectangle(100,100,150,150,vex::color::red);
    while(1)    
    {      
        now_angle=return_angle();    
        if (target_degree>180 && now_angle<180) now_angle+=360;  
        if (target_degree<180 && now_angle>180) now_angle-=360;  

        err_now=target_degree-now_angle;  
        EI=EI+err_now;                    
        if(fabs(err_now)>15)EI=0;       
        ED=err_now-err_last;            //ed

        output=Kp*err_now+Ki*EI+Kd*ED;  
        if(fabs(output)>max_speed) output=sgn(output)*max_speed; 
        just_right_spin(output);     
        err_last=err_now;              
        vex::task::sleep(sampletime);   
        if(fabs(err_now)>2) T2.clear();
        if((T2.time())>150)//200        
        {
            stop(brake);
            //inertial_clear();
            break;
        }

    }
    Brain.Screen.drawRectangle(100,100,150,150,vex::color::green);  
    stop(brake);    
}

void act_control(int pos)
{
    if(pos==0)
    {
        zhua.set(true);
    }
    else if(pos==1)
    {
        zhua.set(false);
    }
    else if(pos==2)
    {
      chan.set(true);
    }
    else if(pos==3)
    {
      chan.set(false);
    }   
}

int some_actions()
    while(1)
    {
      wait(10,msec);
      switch(act)
      {
        case 0:break;
        case 1:act_control(0);act=0;break;
        case 2:act_control(1);act=0;break;
        case 3:act_control(2);act=0;break; 
        case 4:act_control(3);act=0;break;         
        default:act=0;break;
      }
    }
    return 0;
}

This is the PID cpp for the inertial sensor controlled drive train, hope it helps

1 Like

Thanks. (20 character limit)