Jair Zambrano / Mbed 2 deprecated CAR

estabilizador

Dependencies:   MMA8451Q PID TSI mbed

Fork of CAR by mauricio duque

main.cpp@1:f22840ee371d, 2018-06-29 (annotated)

Committer:
JairZambrano97
Date:
Fri Jun 29 17:15:42 2018 +0000
Revision:
1:f22840ee371d
Parent:
0:178159bf8a9c 
estabilizador

Who changed what in which revision?

UserRevisionLine numberNew contents of line
duckmao 0:178159bf8a9c 1 #include "mbed.h"
duckmao 0:178159bf8a9c 2 #include "TSISensor.h"
duckmao 0:178159bf8a9c 3
duckmao 0:178159bf8a9c 4 #include "PID.h"
duckmao 0:178159bf8a9c 5 #define RATE 0.1
duckmao 0:178159bf8a9c 6
duckmao 0:178159bf8a9c 7 #include "MMA8451Q.h"
duckmao 0:178159bf8a9c 8
duckmao 0:178159bf8a9c 9 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
duckmao 0:178159bf8a9c 10 PinName const SDA = PTE25;
duckmao 0:178159bf8a9c 11 PinName const SCL = PTE24;
duckmao 0:178159bf8a9c 12 #elif defined (TARGET_KL05Z)
duckmao 0:178159bf8a9c 13 PinName const SDA = PTB4;
duckmao 0:178159bf8a9c 14 PinName const SCL = PTB3;
duckmao 0:178159bf8a9c 15 #elif defined (TARGET_K20D50M)
duckmao 0:178159bf8a9c 16 PinName const SDA = PTB1;
duckmao 0:178159bf8a9c 17 PinName const SCL = PTB0;
duckmao 0:178159bf8a9c 18 #else
duckmao 0:178159bf8a9c 19 #error TARGET NOT DEFINED
duckmao 0:178159bf8a9c 20 #endif
duckmao 0:178159bf8a9c 21
duckmao 0:178159bf8a9c 22 #define MMA8451_I2C_ADDRESS (0x1d<<1)
duckmao 0:178159bf8a9c 23
duckmao 0:178159bf8a9c 24 TSISensor tsi;
duckmao 0:178159bf8a9c 25 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
duckmao 0:178159bf8a9c 26
duckmao 0:178159bf8a9c 27 //Kc, Ti, Td, interval
duckmao 0:178159bf8a9c 28 PID controller(1.0, 0.0, 0.0, RATE);
duckmao 0:178159bf8a9c 29
duckmao 0:178159bf8a9c 30 int fordward = 1;
duckmao 0:178159bf8a9c 31 int backward = 0;
duckmao 0:178159bf8a9c 32 int direction;
duckmao 0:178159bf8a9c 33 int offSystem;
duckmao 0:178159bf8a9c 34
duckmao 0:178159bf8a9c 35 PwmOut pwmWheelL(D8);
duckmao 0:178159bf8a9c 36 PwmOut pwmWheelR(D5);
duckmao 0:178159bf8a9c 37
duckmao 0:178159bf8a9c 38 DigitalOut directWheelL(D10);
duckmao 0:178159bf8a9c 39 DigitalOut directWheelR(D12);
duckmao 0:178159bf8a9c 40
duckmao 0:178159bf8a9c 41 int main()
duckmao 0:178159bf8a9c 42 {
duckmao 0:178159bf8a9c 43 //Analog input from 0.0 to 3.3V
JairZambrano97 1:f22840ee371d 44 controller.setInputLimits(0.0, 0.40);
duckmao 0:178159bf8a9c 45 //Pwm output from 0.0 to 1.0
JairZambrano97 1:f22840ee371d 46 controller.setOutputLimits(0.0, 4.5);
duckmao 0:178159bf8a9c 47 //If there's a bias.
duckmao 0:178159bf8a9c 48 controller.setBias(0.3);
duckmao 0:178159bf8a9c 49 controller.setMode(1);
duckmao 0:178159bf8a9c 50 //We want the process variable to be 1.7V
JairZambrano97 1:f22840ee371d 51 controller.setSetPoint(0.20);
duckmao 0:178159bf8a9c 52
duckmao 0:178159bf8a9c 53 // directWheelL = fordward;
duckmao 0:178159bf8a9c 54 // directWheelR = fordward;
duckmao 0:178159bf8a9c 55
duckmao 0:178159bf8a9c 56 pwmWheelL.period(0.001);
duckmao 0:178159bf8a9c 57 pwmWheelR.period(0.001);
duckmao 0:178159bf8a9c 58
duckmao 0:178159bf8a9c 59 printf("MMA8451 ID: %d\n", acc.getWhoAmI());
duckmao 0:178159bf8a9c 60
duckmao 0:178159bf8a9c 61 while (true) {
duckmao 0:178159bf8a9c 62 float x, y, z;
duckmao 0:178159bf8a9c 63 x = abs(acc.getAccX());
JairZambrano97 1:f22840ee371d 64 y = abs(acc.getAccY());
duckmao 0:178159bf8a9c 65 z = abs(acc.getAccZ());
duckmao 0:178159bf8a9c 66
duckmao 0:178159bf8a9c 67 //pwmWheelL = 1.0 - tsi.readPercentage();
duckmao 0:178159bf8a9c 68 //pwmWheelR = 1.0 - tsi.readPercentage();
duckmao 0:178159bf8a9c 69 // pwmWheelL = tsi.readPercentage();
duckmao 0:178159bf8a9c 70 // pwmWheelR = tsi.readPercentage();
duckmao 0:178159bf8a9c 71
duckmao 0:178159bf8a9c 72 if(acc.getAccY()<0){
duckmao 0:178159bf8a9c 73 direction = fordward;
duckmao 0:178159bf8a9c 74 }else{
duckmao 0:178159bf8a9c 75 direction = backward;
duckmao 0:178159bf8a9c 76 }
duckmao 0:178159bf8a9c 77 directWheelL = direction;
duckmao 0:178159bf8a9c 78 directWheelR = direction;
duckmao 0:178159bf8a9c 79 //Update the process variable.
duckmao 0:178159bf8a9c 80 controller.setProcessValue(y);
duckmao 0:178159bf8a9c 81 //Set the new output.
duckmao 0:178159bf8a9c 82 if(tsi.readPercentage()>0.00){
duckmao 0:178159bf8a9c 83 pwmWheelL = controller.compute();
duckmao 0:178159bf8a9c 84 pwmWheelR = pwmWheelL;
duckmao 0:178159bf8a9c 85 }else{
duckmao 0:178159bf8a9c 86 pwmWheelL = 0;
duckmao 0:178159bf8a9c 87 pwmWheelR = 0;
duckmao 0:178159bf8a9c 88 }
duckmao 0:178159bf8a9c 89 // pwmWheelL = controller.compute();
duckmao 0:178159bf8a9c 90 // pwmWheelR = pwmWheelL;
duckmao 0:178159bf8a9c 91 //Wait for another loop calculation.
duckmao 0:178159bf8a9c 92 wait(RATE);
duckmao 0:178159bf8a9c 93
duckmao 0:178159bf8a9c 94 printf("X: %1.2f, Y: %1.2f, Z: %1.2f, Touch: %1.2f, PID: %1.2f\n", x, y, z, tsi.readPercentage(), controller.compute());
duckmao 0:178159bf8a9c 95 }
duckmao 0:178159bf8a9c 96 }