File content as of revision 2:2e7ed3a34dd1:

#include "mbed.h"
#include "SerialRPCInterface.h"
#include "MPU6050.h"
#include "TSL1401CL.h"
#include "QEI_simple.h"
#include <list>

// Sensor pins
#define clk p16
#define si p17
#define adc p18
#define revs_per_count 894

void steerImprovedPointTurns(int8_t line_pos);
void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d);
void readCamera(Arguments* input, Reply* output);

SerialRPCInterface SerialRPC(USBTX, USBRX, 115200);
 
float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, duty_cycle_left, duty_cycle_right;
int line_pos, line_pos_previous, line_lost_time;
//int r_enc, l_enc, l_phase, r_phase;
TSL1401CL cam(clk, si, adc);

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<float> rpc_left_duty_cycle(&duty_cycle_left, "duty_cycle_left");
RPCVariable<float> rpc_right_duty_cycle(&duty_cycle_right, "duty_cycle_right");
RPCVariable<int> rpc_line_pos(&line_pos, "line_pos");
//RPCFunction rpc_camera_reading(&readCamera, "readCamera");
//RPCVariable<int>   rpc_r_enc(&r_enc, "r_enc");
//RPCVariable<int> rpc_l_enc(&l_enc, "l_enc");
//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;
        }
    }
    //while(!cam.integrationReady());
    //cam.read();
    
    setLEDs(0, 0, 0, 1);
    duty_cycle_left = 0; duty_cycle_right = 0;
    while(1) {
        /***** Read line sensor *****/
        //cam.read();
        wait_ms(10);
        m1_fwd.write(duty_cycle_left);
        m2_fwd.write(duty_cycle_right);
        //r_enc=r_wheel.getPulses();
        //l_enc=l_wheel.getPulses();
        
        if (imu_ready) {            
            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];
            }
        }
        /*
        l_phase = l_enc % revs_per_count; r_phase = r_enc % revs_per_count;
        if ((l_phase < revs_per_count / 2) && (r_phase < revs_per_count / 2)) {
            setLEDs(1, 0, 1, 0);
        } else if ((l_phase < revs_per_count / 2) && (r_phase > revs_per_count / 2)) {
            setLEDs(1, 0, 0, 1);
        } else if ((l_phase > revs_per_count / 2) && (r_phase < revs_per_count / 2)) {
            setLEDs(0, 1, 1, 0);
        } else {
            setLEDs(0, 1, 0, 1);
        }*/
    }
}

void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  led1 = a;
  led2 = b;
  led3 = c;
  led4 = d;
}
/*
void readCamera(Arguments* input, Reply* output){    
    uint32_t pos = input->getArg<int>();       
    int ans = 0;
    setLEDs(1, 0, 0, 1);
    //if (pos < 128u){
    ans = cam.getData(pos);
    //}   
    output->putData(ans);
    setLEDs(0, 1, 1, 0);
}*/