File content as of revision 1:7b8696baf8ff:

#include "mbed.h"
#include "SerialRPCInterface.h"
#include "MPU6050.h"
#include "TSL1401CL.h"
#include "QEI.h"
void steerImprovedPointTurns(int8_t line_pos);
void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d);
SerialRPCInterface SerialRPC(USBTX, USBRX, 115200);
 
float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, left_pwm, right_pwm;
int line_pos, line_pos_previous, line_lost_time;

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
PwmOut m1_fwd(p21);
PwmOut m1_back(p22);
PwmOut m2_fwd(p23);
PwmOut m2_back(p24);
RPCVariable<float> rpc_accel_x(&accel_x, "accel_x");
RPCVariable<float> rpc_accel_y(&accel_y, "accel_y");
RPCVariable<float> rpc_accel_z(&accel_z, "accel_z");
RPCVariable<float> rpc_gyro_x(&gyro_x, "gyro_x");
RPCVariable<float> rpc_gyro_y(&gyro_y, "gyro_y");
RPCVariable<float> rpc_gyro_z(&gyro_z, "gyro_z");
RPCVariable<int> rpc_line_pos(&line_pos, "line_pos");

//QEI l_wheel (p29, p30, NC, 624);
//QEI r_wheel (p11, p12, NC, 624);
 
MPU6050 mpu6050;

#define CAM_INTEGRATION_TIME 80

// Higher line threshold -> the sensor will only recognize larger changes in
// brightness as a line edge
#define LINE_THRESHOLD 10
#define LINE_PRECISION 2
#define LINE_CROP_AMOUNT 4

// These constants define the base pwm across the motors and how much the
// controller
// adjusts based on position of the line relative to the sensor
#define SPEED_PWM 0.2
#define TURN_SENS_INNER 1.5F
#define TURN_SENS_OUTER 0.5F

// Defines data
#define LINE_HIST_SIZE 1000
#define LINE_END_TIME 2500

// Sensor pins
#define clk p16
#define si p17
#define adc p18
 
int main() {
    /********** SENSOR SETUP **********/
    setLEDs(1, 1, 1, 1);
        
    //Set up I2C
    i2c.frequency(400000);  // use fast (400 kHz) I2C
    
    volatile bool imu_ready = false;
    
    wait_ms(100);
    
    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
    
    if (whoami == 0x68) // WHO_AM_I should always be 0x68
    {
        mpu6050.MPU6050SelfTest(SelfTest);
        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
            mpu6050.initMPU6050();
            mpu6050.getAres();
            mpu6050.getGres();
            imu_ready = true;
        }
    }
    
    setLEDs(0, 0, 0, 1);
        
    line_lost_time = 0;
    while(1) {
        wait_ms(10);
        m1_fwd.write(left_pwm);
        m2_fwd.write(right_pwm);
        /***** Read line sensor *****/
        
        if (imu_ready) {            
            setLEDs(0, 1, 0, 0);
            if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
                mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
                mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
 
                // Now we'll calculate the accleration value into actual g's
                accel_x = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
                accel_y = (float)accelCount[1]*aRes - accelBias[1];   
                accel_z = (float)accelCount[2]*aRes - accelBias[2];  
               
                // Calculate the gyro value into actual degrees per second
                gyro_x = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
                gyro_y = (float)gyroCount[1]*gRes - gyroBias[1];  
                gyro_z = (float)gyroCount[2]*gRes - gyroBias[2];
            }
        }
    }
}

void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  led1 = a;
  led2 = b;
  led3 = c;
  led4 = d;
}