File content as of revision 8:a1d19a540c83:

#include "mbed.h"
#include "math.h"
#include "TextLCD.h"
#define LENOFBOT 2.5
#define LENOFSENSOR 0.394
#define ERROR 0.5
//obj for distance sensor
 
Serial pc(USBTX, USBRX);
DigitalOut solenoid(p11);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
PwmOut Rwheel(p21);
PwmOut Lwheel(p22);
PwmOut trigger(p24);
PwmOut trigger_b(p23);
InterruptIn echoFront(p12,PullDown);
InterruptIn echoBack(p13,PullDown);
Timer timer;
Timer timer_b;
//functions for distance sensor
volatile float front_reading;
volatile float back_sensor;
void stop_back (void);
void start_back(void);    
void stop (void);
void start(void);
void forward(void);
void backward(void);
void rotateRight(void);
void rotateLeft(void);
void stopMotors(void);
//functions for MPU
Serial Ardu(p9,p10);
void inline MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX); 
float inline filteredAngle(float *pitch, float *gyrX);

//LCD
I2C i2c_lcd(p28,p27);
TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD16x2);


int main() {
    int16_t accy;
    int16_t accz;
    int16_t gyr;
    float angleX =0;
    float samplerate = 9600.0;
    float finalAx=0;
    float pitch =0;
    float offset =0;
    float center_distance;
 //   float gyr_with_ofset =0;
    int k =1; // this is for getting the initial angle the first time around so we can start at zero
 //   int j=0;
    
    //distance sensor stuff
    echoFront.rise(&start);
    echoFront.fall(&stop);
    echoBack.rise(&start_back);
    echoBack.fall(&stop_back);
    trigger.period(0.06);
    trigger_b.period(0.06);
    int pi[]= {3,1,4,1,5,9,2,6,5,3,5,8,9,7,9,3,2,3,8,4,6,2,6,4,3,3,8,3,2,7,9,5,0,2,8,8,4,1,9,7,1,6,9,3,9,9,3,7,5,1,0,5,8,2,0,9,7,4,9,4,4,5,9,2,3,0,7,8,1,6,4,0,6,2,8,6,2,0,8,9,9,8,6,2,8,0,3,4,8,2,5,3,4,2,1,1,7,0,6,7,9};
    int currentdigit =0;
    int target_front_distance;
    int back_distance;
    
    //motors
    Lwheel.period_us(21700);
    Rwheel.period_us(21700);
    while(1) {
        
        led1=0;
        led2=0;
        led3=0;
        led4=0;
        trigger.pulsewidth_us(10);
        trigger_b.pulsewidth_us(10);
        target_front_distance = (3*pi[currentdigit])+9;
        back_distance= 27-target_front_distance;
        MPU6050Values(&Ardu,&accy,&accz,&gyr, &samplerate, &k, &offset,&pitch, &angleX);
        finalAx = filteredAngle(&pitch, &angleX);
    
       lcd.printf("A: %.2f\n\r",finalAx);
       center_distance = (front_reading/2)+LENOFBOT+LENOFSENSOR;
       if(finalAx>+0.5){
            rotateLeft();
        }else if(finalAx<-0.5){
             rotateRight();
            }
        if(center_distance<(target_front_distance+ERROR)){
            pc.printf("moving back distance read: %.3f digit: %d\n\r",center_distance,target_front_distance);
            backward();
            //if(((back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance-0.1||(back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance+0.1))
            
        }
        
        if(center_distance>(target_front_distance-ERROR)){
            pc.printf("moving forward distance read: %.3f should be %d\n\r",center_distance,target_front_distance);
            forward();
            //if((back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance-0.1||(back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance+0.1)
            
        }
        
        if(center_distance>(target_front_distance-ERROR)&&center_distance<(target_front_distance+ERROR)){
            stopMotors();
            pc.printf("stopped distance is: %.3f should be %d\n\r",center_distance,target_front_distance);
            solenoid =1;
            wait_ms(100);
            solenoid =0;
            currentdigit++;
        }
    }
}

void start(void){
    timer.start();
    led4=1;
    led3=0;
    }
void stop (void){
    timer.stop();
    front_reading = (float)timer.read()*13504;
    timer.reset();
    led3=1;
    led4=0;
    }
void start_back(void){
    timer_b.start();
    led2=1;
    }
void stop_back (void){
    timer_b.stop();
    back_sensor = (float)timer_b.read()*13480;
    timer_b.reset();
    led1=1;
    }
    
    
inline void MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX){
    uint8_t charArr[6];
    for(int i =0; i<6;i++)
        charArr[i]=Ardu->getc();
    
    (*accy)= charArr[0]<<8|charArr[1];
    (*accz)= charArr[2]<<8|charArr[3];
    (*gyr)= charArr[4]<<8|charArr[5];
    float thing=(*accy)/(*accz);
    (*pitch) = atan(thing);
    if(*k){
        (*offset) = (*gyr);//gets the offset or starting point the first time around
        *k=0;
        }
        
    (*angleX) += ((*gyr) - (*offset))/(*samplerate);
    }

inline float filteredAngle(float *pitch, float *angleX){
    return ((*pitch)*(0.5)+(*angleX)*(0.95))+0.25;
    }
    
    
void backward(void){
    Lwheel.pulsewidth_us(1550);
    Rwheel.pulsewidth_us(1450);
    }
void forward(void){
    Lwheel.pulsewidth_us(1450);
    Rwheel.pulsewidth_us(1550);
    }
void rotateRight(void){
    Lwheel.pulsewidth_us(1700);
    Rwheel.pulsewidth_us(1700);
    }
void rotateLeft(void){
    Lwheel.pulsewidth_us(1300);
    Rwheel.pulsewidth_us(1300);
    }
void stopMotors(void){
    Lwheel.pulsewidth_us(1500);
    Rwheel.pulsewidth_us(1500);
    }