File content as of revision 0:9d2ceb7b8ec9:

#include "mbed.h"                                              
#include "LSM9DS0.h"                                          
#include "math.h"                                               

LSM9DS0 imu(p9, p10, 0x6B, 0x1D); 
Serial pc(USBTX, USBRX);
Ticker control;  // create Ticker for control algorithm
Ticker db;

//debugging
int debug=0;
int wt = 500;
DigitalOut led1(LED1);

//motor variables
PwmOut left_motor_speed(p21); //left motor pwm A
PwmOut right_motor_speed(p22); //right motor pwm B
DigitalOut left_fwd_enable(p23); //fwd enable of left/A motor
DigitalOut left_rev_enable(p24); //rev enable of left/A motor
DigitalOut right_fwd_enable(p26); //fwd enable or right/B motor
DigitalOut right_rev_enable(p25); //rev enable of right/B motor

//angle variables
float accelerometer_angle =0;
float gyro_data =0;
float gyro_angle=0;
float current_angle =0;
float angle_offset = 0;
int i=0;

//control variables
float Error=0; // I had to capitalize this for some reason unlike the other vars
float kp=.5;
float ki=.01;
float kd= 0;
float setpoint=0; //reference angle
float angle=0;
float integral=0;
float derivative = 0;
float dt= 0.01; //time period 1/100 (s)
float previous_error =0;
float actuator_input =0;
int ready = 0;
float maximum_angle=15;
float SCALAR = 0;

void initialiation (void)
{
    //with the "initialization" in the control loop, we may not use this
    //this is where offsets and sensors are set up
    //need to hold robot balanced at this point
    imu.readGyro();
    angle_offset= float(imu.gy); //maybe take avg. like the other team did in a for loop   
}

void MotorDriver (float speed)
{
// if speed > 0 then move robot left
// if speed < 0 then move robot right
    if ( speed>0 )
    {
    //move both motors in same direction
    right_rev_enable = 0;
    right_fwd_enable = 1;
    left_fwd_enable = 0;
    left_rev_enable = 1;
    right_motor_speed = speed;
    left_motor_speed = speed;
    //pc.printf("moving f\n");
    }
    else if ( speed <0 )
    {
    //move both motors in same direction
    right_fwd_enable = 0;
    right_rev_enable = 1;
    left_rev_enable = 0;
    left_fwd_enable = 1;
    right_motor_speed = -speed;
    left_motor_speed= -speed;
    //pc.printf("moving r\n");
    }
    else 
    {
    right_fwd_enable = 0;
    right_rev_enable = 0;
    left_rev_enable = 0;
    left_fwd_enable = 0;
    right_motor_speed = speed;
    left_motor_speed = speed;
    //pc.printf("not moving\n");
    }
}

float calcSCALAR(float degrees)
{
    //give it a degree such as 10 degrees, anything past that is the same as 10 degrees
    Error = 0 - degrees; //this is the maximum error
    integral = integral + (Error*dt); //Forward Euler approximation
    derivative = (Error - previous_error) / dt; //change wrt time
    previous_error = Error; 
    SCALAR = ( (Error*kp) + (integral*ki) + (derivative*kd) );
    pc.printf("here in scalar calc and error is %f and integral is %f and derivative is %f \n", Error, integral, derivative);
    Error=0;
    integral=0;
    derivative=0;
    previous_error=0;    
    return 1;
}

void controlalgorithm (void)
{
    if (i< wt) //gives you time to balance it upon reset; this is like an initialization code for the imu;
    {
        //determine current angle using a complementary filter found online http://www.pieter-jan.com/node/11
        //place +z-axis up, + x-axis right, and +y-axis away from the back of the robot when robot can move left to right 
        imu.readGyro();
        gyro_data= imu.gy;
        imu.readAccel(); 
        accelerometer_angle = atan2(imu.ax, imu.az) * 180 / 3.14159; //angle in degrees; positive angle is CCW, negative angle is CW; this gives the angle as determined by the accelerometer
        current_angle = 0.98 * (current_angle + gyro_data * dt) + 0.02 * (accelerometer_angle);
        i=i+1;      
    }
    
    //if the SCALAR has been calculated and the initilization complete, start controlling
    if ( ready ==1 )
    {
        imu.readGyro();
        gyro_data= imu.gy; //maybe negative?? I don't think so
        imu.readAccel(); 
        accelerometer_angle = atan2(imu.ax, imu.az) * 180 / 3.14159; //angle in degrees; positive angle is CCW, negative angle is CW
        current_angle = 0.98 * (current_angle + gyro_data * dt) + 0.02 * (accelerometer_angle);    
            
        //maybe take into account integral windup
        Error = setpoint - current_angle; //determine the difference in angle
        integral = integral + (Error*dt); //Forward Euler approximation
        derivative = (Error - previous_error) / dt; //change wrt time
        previous_error = Error; 
        actuator_input = ( (Error*kp) + (integral*ki) + (derivative*kd) ) / SCALAR; //we should scale this???? and write in terms of a PWM value [-1,1]; be careful, SCALAR is negative
        MotorDriver(actuator_input);
    }
    
}

void debugout (void)
{
    //pc.printf ("setpoint is %f\n", setpoint);
    pc.printf ("actuator_input is %f\n", actuator_input);
    //pc.printf("Speed is: %f, angle is: %f\n\n",speed,current_angle);
}

main()
{
    pc.printf("starting...\n");
    uint16_t status = imu.begin();
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
    pc.printf("Should be 0x49D4\n\n");
    
    SCALAR= calcSCALAR(maximum_angle); //scale the actuator inputl depends on lp, kd, and ki, and maximum angle allowed
    control.attach(&controlalgorithm, dt); //check frequency, 100 Hz
    if (debug==1)
    {
        db.attach(&debugout, 0.1);
    }
    
    while(1)
    {
        //put some calcs here if needed; need 0.01 seconds at least
        if (i==wt)
        {
           // setpoint=current_angle / 1000; //take first 1000 samples for current angle then avg. it
            setpoint=current_angle; 
            ready=1;
            i=i+1;
            pc.printf ("i is %d in top if statement\n\n\n\n", i);
        }
        
        //if (debug == 1 && i>20 )
//        {
//            //probably need some kind of wait statement; will a wait statement affect the timer interrupt? no, i checked
//            pc.printf("accel: x: %f y: %f z: %f\n",imu.ax,imu.ay,imu.az);
//            pc.printf("gyro: x: %f y: %f z: %f\n",imu.gx,imu.gy,imu.gz);
//            pc.printf ("current_angle is %f\n", current_angle);
//            pc.printf ("setpoint is %f\n", setpoint);
//            pc.printf ("accelerometer_angle is %f\n", accelerometer_angle);
//            pc.printf ("gyro_angle is %f\n", gyro_angle);
//            pc.printf ("actuator_input is %f\n", actuator_input);
//            pc.printf ("SCALAR is %f \n", SCALAR);
//            pc.printf ("i is %d\n\n\n\n", i);
//            wait(2);
//        }
    }
}