Heres our PID code so far...the constants u multiply by really have to be tweaked once the actual robot is built
! ... P+I+D should not be over 127 or under -127...so i'll have to definitely calibrate it A LOT
IF( any one spots obvious mistakes ...(not like syntax error type mistakes) ){please tell me...}
Code:
////get joystick input///////////////
p2_y=armin;
targetv=armin; //just temporary...
//////get gyro input conv to 8bit
(int)gyroin10bit=Get_Analog_Value(rc_anlg_in01);
gyroval=gyroin10bit/4;
////convert gyro values to usable values....
armaccel=gyroval-127;
///integrate
realv=(armaccel+realv)/time;
//////////////
//////////Calculate the proportional.....
if((hicutoff+targetv)>realv>(locutoff+targetv))
{
armout=(targetv-realv)*proportion; /// as the real gets closer to target pwm addition is lowered...
/// this is basically "Gain"
}
/////////////calculate the integral of the error....
error=realv-targetv;
errorint=(error+errorint/time)*intprop;
/////calculate the derivative of the error;
derivative=(lasterror-error)*devprop;
///////////////////Output;////////////////////
if(tripped!=1)
{
pwm03=armout+errorint+derivative+127;
}
///////update time
time++;
////update error
lasterror=error;
////motor heating control....
///basically take current sensor values and make sure not too much
//is being drawn or else it will let down on the motors
(int)current=(Get_Analog_Value(rc_anlg_in02))/4;
///+127 = 100 amps...... so + or - 50 = 40 AMPS!!! I set it to "trip" at 27 amps
/// since we have 30 amps breakers ... so that you don't trip the actual breaker.!
//// divide current by .79 to get anlgreading-127~! so 100/.79+127=254!
if(current>161 || <93)
{
pwm03=p2_y;
tripped=1;
}
I also added some code in to ensure that if we have a large weight on the arm at a not so great angle the motors won't over heat and trip the breaker....
Am I doing the Proportional correctly? It says Gain= Input / Ouput ... but in stead i just compare the target to the actual...thats not exactly the same but should give me the result i'm looking for (to lower speed when target is closer to actual)?? Correct me if i'm wrong please....