File content as of revision 0:8adde5eaea5a:

#ifndef ACTIONS_H
#define ACTIONS_H

PwmOut mypwm(PWM_OUT);
DigitalOut myled(LED1);
//Left Motor
PwmOut m1F(PC_7);
PwmOut m1B(PB_10);
//Right Motor
PwmOut m2F(PA_7);
PwmOut m2B(PB_6);
//Left Encoder
AnalogIn lFront(PB_3);
AnalogIn lBack(PA_15);
//Right Encoder
AnalogIn rFront(PA_1);
AnalogIn rBack(PC_14);

Timer time;

struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?)
  pid(){
        integral = 0.08f;
        prev = 0.0f;
        kp = 1.0f; //the ks should be negative to counteract error
        ki = 0.0f;
        kd = 0.8f;
      }
  float integral;
  int prev;
  float kp; //the ks should be negative to counteract error
  float ki;
  float kd;
};

void resetPid(struct pid *e) {
  e->integral = 0.0f;
  e->prev = 0.0f;
}

float getFix(struct pid *e, float error) {
  //float d = (error - e->prev)/DT;
  e->integral += error * DT;
  e->prev = error;
}


void turnleft_loop(float lencoder, float rencoder){ //4096 per turn of wheel
    while(lencoder < 12861.0f){
        float error = lencoder - 12861.0f;
        float delta_motor = getFix(lturn, error);
        m1F.period(0.01f);
        m1F = m1basespeed + delta_motor;
    }
    while(rencoder < 12861f){
        float error = rencoder - 12861f;
        float delta_motor = getFix(rturn, error);
        m2B.period(0.01f);
        m2B = m2basespeed + delta_motor;
    }
}













#endif