File content as of revision 1:f22840ee371d:

#include "mbed.h"
#include "TSISensor.h"

#include "PID.h"
#define RATE 0.1

#include "MMA8451Q.h"

#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
  PinName const SDA = PTE25;
  PinName const SCL = PTE24;
#elif defined (TARGET_KL05Z)
  PinName const SDA = PTB4;
  PinName const SCL = PTB3;
#elif defined (TARGET_K20D50M)
  PinName const SDA = PTB1;
  PinName const SCL = PTB0;
#else
  #error TARGET NOT DEFINED
#endif

#define MMA8451_I2C_ADDRESS (0x1d<<1)

TSISensor tsi;
MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);

//Kc, Ti, Td, interval
PID controller(1.0, 0.0, 0.0, RATE);

int fordward = 1;
int backward = 0;
int direction;
int offSystem;

PwmOut pwmWheelL(D8);
PwmOut pwmWheelR(D5);

DigitalOut directWheelL(D10);
DigitalOut directWheelR(D12);

int main()
{
    //Analog input from 0.0 to 3.3V
    controller.setInputLimits(0.0, 0.40);
    //Pwm output from 0.0 to 1.0
    controller.setOutputLimits(0.0, 4.5);
    //If there's a bias.
    controller.setBias(0.3);
    controller.setMode(1);
    //We want the process variable to be 1.7V
    controller.setSetPoint(0.20);
    
   // directWheelL = fordward;
//    directWheelR = fordward;
    
    pwmWheelL.period(0.001);
    pwmWheelR.period(0.001);
    
    printf("MMA8451 ID: %d\n", acc.getWhoAmI());
   
    while (true) {
        float x, y, z;
        x = abs(acc.getAccX());
        y = abs(acc.getAccY());
        z = abs(acc.getAccZ());
        
        //pwmWheelL = 1.0 - tsi.readPercentage();
        //pwmWheelR = 1.0 - tsi.readPercentage();
//        pwmWheelL = tsi.readPercentage();
//        pwmWheelR = tsi.readPercentage();

        if(acc.getAccY()<0){
            direction = fordward;
        }else{
            direction = backward;
        }
        directWheelL = direction;
        directWheelR = direction;
        //Update the process variable.
        controller.setProcessValue(y);
        //Set the new output.
        if(tsi.readPercentage()>0.00){
            pwmWheelL = controller.compute();
            pwmWheelR = pwmWheelL;
        }else{
            pwmWheelL = 0;
            pwmWheelR = 0;
        }
//        pwmWheelL = controller.compute();
//        pwmWheelR = pwmWheelL;
        //Wait for another loop calculation.
        wait(RATE);
    
        printf("X: %1.2f, Y: %1.2f, Z: %1.2f, Touch: %1.2f, PID: %1.2f\n", x, y, z, tsi.readPercentage(), controller.compute());
    }
}