Professional Documents
Culture Documents
int
int
int
int
int
int
float angle = 0;
float angle_old = 0;
float angle_dydx = 0;
float angle_integral = 0;
float balancetorque = 0;
float rest_angle = 0;
float currentspeed = 0;
int steeringZero = 0;
int steering = 0;
int steeringTemp = 0;
float p = 8; //2
float i = 0; //0.005
float d = 1300; //1000
float gyro_integration = 0;
float xZero = 0;
int gZero = 445; //this is always fixed, hence why no initialisation routine
unsigned long time, oldtime;
int pwmL;
int pwmR;
boolean over_angle = 0;
void setup() {
unsigned int i = 0;
unsigned long j = 0; //maximum possible value of j in routine is 102300 (100*102
3)
pinMode(ledPin, OUTPUT); // declare the ledPin as an OUTPUT
Serial.begin(115200);
analogReference(EXTERNAL);
//---------------------------------------------------TCCR1B = TCCR1B & 0b11111000 | 0x01;
analogWrite(pwmPinL,127);
analogWrite(pwmPinR,127);
digitalWrite(enPin,HIGH);
pinMode(enPin,OUTPUT);
digitalWrite(enPin,LOW);
//----------------------------------------------------delay(100);
for (i = 0; i < j =" j" steeringzero =" analogRead(steerPin);" xzero =" j/100;"
oldtime =" micros();" time =" micros();">= (oldtime+5000)){
oldtime = time;
calculateAngle();
steering = (analogRead(steerPin) - steeringZero)/(15+(abs(angle)*8));
//-----OVER ANGLE PROTECTION-----