Revision 0:34c1f05d8d2c, committed 2019-11-16

Comitter:

jpapratov

Date:

Sat Nov 16 10:14:58 2019 +0000

Child:

1:024485d1c677

Commit message:

Test.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04_Library/HCSR04.cpp	Sat Nov 16 10:14:58 2019 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include "HCSR04.h"
+
+HCSR04::HCSR04(PinName echoPin, PinName triggerPin) : echo(echoPin), trigger(triggerPin) {
+    init();
+}
+
+void HCSR04::init() {
+    distance = -1;      // initial distance
+    minDistance = 2;
+    maxDistance = 400;
+    newDataReady = timerStarted = false;
+}
+
+void HCSR04::startTimer() {
+    if (!timerStarted) {
+        timer.start(); // start the timer
+        timerStarted = true;
+        echoTimeout.attach_us(this, &HCSR04::stopTimer, 25000); // in case echo fall does not occur
+        echo.fall(this, &HCSR04::stopTimer);
+        echo.rise(NULL);
+    }
+}
+
+void HCSR04::stopTimer() {
+    timer.stop(); // stop the timer
+    if (timerStarted) {
+        distance = timer.read() * 1e6 / 58;
+        if (distance < minDistance)
+            distance = minDistance;
+        if (distance > maxDistance)
+            distance = maxDistance;
+        newDataReady = true;
+    }
+    timer.reset();
+    timerStarted = false;
+    echoTimeout.detach();
+    echo.fall(NULL);
+}
+
+void HCSR04::turnOffTrigger() {
+    trigger = 0; 
+}
+
+void HCSR04::startMeasurement() {
+    trigger = 1;
+    triggerTimeout.attach_us(this, &HCSR04::turnOffTrigger, 10);
+    echo.rise(this, &HCSR04::startTimer);
+    newDataReady = false;
+}
+
+float HCSR04::getDistance_cm() {
+    newDataReady = false;
+    return distance;
+}
+
+float HCSR04::getDistance_mm() {
+    newDataReady = false;
+    return distance * 10;
+}
+
+bool HCSR04::isNewDataReady() {
+    return newDataReady;
+}
+
+void HCSR04::setRanges(float minRange, float maxRange) {
+    if (minRange < maxRange) {
+        if (minRange >= 2 && minRange < 400) // bug from revs. 4 and 5 corrected
+            minDistance = minRange;
+        if (maxRange <= 400)
+            maxDistance = maxRange;
+    }
+}
+
+float HCSR04::getMinRange() {
+    return minDistance;
+}
+
+float HCSR04::getMaxRange() {
+    return maxDistance;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04_Library/HCSR04.h	Sat Nov 16 10:14:58 2019 +0000
@@ -0,0 +1,103 @@
+#ifndef HCSR04_H
+#define HCSR04_H
+
+/** A distance measurement class using ultrasonic sensor HC-SR04.
+ *  
+ * Example of use:
+ * @code
+ * #include "mbed.h"
+ * #include "HCSR04.h"
+ *
+ * Serial pc(USBTX, USBRX);
+ * Timer timer;
+ *
+ * int main() {
+ *     HCSR04 sensor(p5, p7);
+ *     sensor.setRanges(10, 110);
+ *     pc.printf("Min. range = %g cm\n\rMax. range = %g cm\n\r",
+ *       sensor.getMinRange(), sensor.getMaxRange());
+ *     while(true) {
+ *         timer.reset();
+ *         timer.start();
+ *         sensor.startMeasurement();
+ *         while(!sensor.isNewDataReady()) {
+ *             // wait for new data
+ *             // waiting time depends on the distance
+ *         }
+ *         pc.printf("Distance: %5.1f mm\r", sensor.getDistance_mm());
+ *         timer.stop();
+ *         wait_ms(500 - timer.read_ms()); // time the loop
+ *     }
+ * }
+ * @endcode
+ */
+class HCSR04 {
+    
+    public:
+    
+    /** Receives two PinName variables.
+     * @param echoPin mbed pin to which the echo signal is connected to
+     * @param triggerPin mbed pin to which the trigger signal is connected to
+     */
+    HCSR04(PinName echoPin, PinName triggerPin);
+    
+    /** Start the measurement. Measurement time depends on the distance.
+     *  Maximum measurement time is limited to 25 ms (400 cm).
+     */
+    void startMeasurement();
+    
+    /** Returns the distance in cm. Requires previous call of startMeasurement().
+     * @returns distance of the measuring object in cm.
+     */
+    float getDistance_cm();
+    
+    /** Returns the distance in mm. Requires previous call of startMeasurement().
+     * @returns distance of the measuring object in mm.
+     */
+    float getDistance_mm();
+    
+    /** Sets the minimum and maximum ranges between the factory values of 2 cm and 400 cm.
+     *  @param minRange Minimum range in cm. Must be between 2 cm and maxRange.
+     *  @param maxRange Maximum range in cm. Must be between minRange and 400 cm.
+     */
+    void setRanges(float minRange, float maxRange);
+    
+    /** Retreives the minimum sensor range set by the user.
+     * @returns the minimum sensor range set by the user in cm.
+     */
+    float getMinRange();
+    
+    /** Retreives the maximum sensor range set by the user.
+     * @returns the maximum sensor range set by the user in cm.
+     */
+    float getMaxRange();
+    
+    /** Checks if the new data is ready.
+     * @returns true if new data is ready, false otherwise.
+     */
+    bool isNewDataReady();
+    
+    private:
+    
+    InterruptIn echo;       // echo pin
+    DigitalOut trigger;     // trigger pin
+    Timer timer;            // echo pulsewidth measurement
+    float distance;         // store the distance in cm
+    float minDistance;      // minimum measurable distance
+    float maxDistance;      // maximum measurable distance
+    Timeout triggerTimeout, echoTimeout;
+    bool newDataReady, timerStarted;
+    
+    /** Start the timer. */
+    void startTimer();
+    
+    /** Stop the timer. */
+    void stopTimer();
+    
+    /** Initialization. */
+    void init();
+    
+    void turnOffTrigger();
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU_Library/MPU.cpp	Sat Nov 16 10:14:58 2019 +0000
@@ -0,0 +1,160 @@
+#include "MPU.h"
+#include "mbed.h"
+
+Senzor::Senzor(PinName Sda, PinName Scl): mpu(Sda, Scl){
+    mpu.frequency(400000);
+    setPowerManagement (0x00);//enable measurement
+
+    GYRO_CONFIG(0x10);//Set the register bits as 00010000 (1000dps full scale)
+
+    ACCEL_CONFIG(0x10);//Set the register bits as 00010000 (+/- 8g full scale range)
+
+    setBW(0x00);//Set Bandwidth 0 -> 256Hz
+}
+
+    char Senzor::testConnection ()
+{
+    char dat;
+    char reg=WHO_AM_I_REG;
+    mpu.write(ADDRESS, &reg, 1, true);
+    mpu.read(ADDRESS, &dat, 1, false);
+    return dat;
+}
+void Senzor::setBW (char command)
+{
+    char dat[2];
+
+    dat[0]=CONFIG_REG;
+    dat[1]=command;
+
+    mpu.write(ADDRESS, dat, 2, false);
+}
+
+void Senzor::setPowerManagement (char command)
+{
+    char dat[2];
+
+    dat[0]=PWR_MGMT_1_REG;
+    dat[1]=command;
+
+    mpu.write(ADDRESS, dat, 2, false);
+
+}
+
+void Senzor::GYRO_CONFIG(char command)
+{
+    char dat[2];
+
+    dat[0] = GYRO_CONFIG_REG; //GYRO_CONFIG register
+    dat[1] = command;
+
+    mpu.write(ADDRESS, dat, 2, false);
+}
+
+void Senzor::ACCEL_CONFIG(char command)
+{
+    char dat[2];
+
+    dat[0] = ACCELERO_CONFIG_REG; //ACCEL_CONFIG register
+    dat[1] = command;
+
+    mpu.write(ADDRESS, dat, 2, false);
+}
+
+void Senzor::getAccel(float *accel)
+{
+    char dat[6];
+    dat[0] = ACCEL_XOUT_H_REG; //ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L
+
+    mpu.write(ADDRESS, &dat[0], 1, true);
+    mpu.read(ADDRESS, dat, 6, false);//krece od 3B -> 40
+
+    accel[0] = short(dat[0] << 8 | dat[1])/ 4096.0 * 9.81;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
+    accel[1] = short(dat[2] << 8 | dat[3])/ 4096.0 * 9.81;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
+    accel[2] = short(dat[4] << 8 | dat[5])/ 4096.0 * 9.81;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
+
+}
+void Senzor::getGyro(float *gyro)//očitavanje žiroskopa i spremanje podataka
+{
+    char dat[6];
+    dat[0] = GYRO_XOUT_H_REG; //GYRO_XOUT_H,GYRO_XOUT_L,GYRO_YOUT_H,GYRO_YOUT_L,GYRO_ZOUT_H,GYRO_ZOUT_L
+
+    mpu.write(ADDRESS, &dat[0], 1, true);
+    mpu.read(ADDRESS, dat, 6, false);//krece od 43 -> 48
+
+    gyro[0] = short(dat[0] << 8 | dat[1])/ 1879.3;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
+    gyro[1] = short(dat[2] << 8 | dat[3])/ 1879.3;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
+    gyro[2] = short(dat[4] << 8 | dat[5])/ 1879.3;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
+}
+void Senzor::getAccAngle(float *angle)//računanje kuteva x i y pomoću akceleracije
+{
+    float temp[3];
+    getAccel(temp);
+    angle[0] = atan (temp[1]/sqrt(pow(temp[0], 2) + pow(temp[2], 2))) * 57.3; //calculate angle x(pitch/roll?) from accellerometer reading
+    angle[1] = atan (-1*temp[0]/sqrt(pow(temp[1], 2) + pow(temp[2], 2))) * 57.3; //calculate angle y(pitch/roll?) from accellerometer reading
+    angle[2] = atan (sqrt(pow(temp[0], 2) + pow(temp[1], 2))/temp[2]) * 57.3;//calculate angle z
+}
+void Senzor::getOffset(float *accOffset, float *gyroOffset, int sample)//računanje odstupanja od stvarne vrijednosti te oduzimanje eventualne pogreške očitanja
+{
+    float gyro[3];
+    float accAngle[3];
+    sample=200;
+
+    for (int i = 0; i < 3; i++) {
+        accOffset[i] = 0.0;     //initialise offsets to 0.0
+        gyroOffset[i] = 0.0;
+    }
+
+    for (int i = 0; i < sample; i++) {
+        getGyro(gyro); //take real life measurements
+        getAccAngle(accAngle);
+
+        for (int j = 0; j < 3; j++) {
+            *(accOffset+j) += accAngle[j]/sample;    //average measurements
+            *(gyroOffset+j) += gyro[j]/sample;
+        }
+        wait_ms (10);    //wait between each reading for accuracy
+    }
+    for (int k = 0; k < 3; k++) {
+        gyro[k] -= gyroOffset[k];       //substract offset values
+        accAngle[k] -= accOffset[k];
+    }
+}
+void Senzor::finalAngle (float *angle)
+{
+    float GyroAng[2];
+    float gyro[3];
+    float accAngle[3];
+    float interval;
+    Timer t;
+
+    interval=t.read();
+    t.reset();
+    getGyro(gyro);           //get gyro value in rad/s
+    getAccAngle(accAngle);   //get angle from accelerometer
+    t.start();
+    // računanje stupnjeva Gyro kuteva rad/s*s=rad*57.3=deg
+    GyroAng[0]=GyroAng[0] + gyro[0]* interval*57.3;
+    GyroAng[1]=GyroAng[1] + gyro[1] * interval*57.3;
+    angle[2]= angle[2] + gyro[2] * interval*57.3; //yaw
+    
+    angle[0]=0.96*GyroAng[0]+0.04*accAngle[0]*25; //pitch
+    angle[1] = 0.96 * GyroAng[1] + 0.04 * accAngle[1]*25; //roll
+
+}
+float Senzor::getTemp()//očitavanje i spremanje temperature
+{
+    char dat[2];
+    float temp;
+
+    dat[0]=TEMP_H_REG;
+
+    mpu.write(ADDRESS, &dat[0], 1, true);
+    mpu.read(ADDRESS, dat, 2, false);//krece od 41 -> 42
+
+    temp =short(dat[0] << 8 | dat[1]);// 0x41 (TEMP_H_REG) & 0x42 (TEMP_H_REG)
+
+    temp=((double)temp+521.0)/340.0+35.0;
+
+    return temp;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU_Library/MPU.h	Sat Nov 16 10:14:58 2019 +0000
@@ -0,0 +1,35 @@
+#ifndef MBED_MPU_H
+#define MBED_MPU_H
+
+#include "mbed.h"
+
+#define ADDRESS (0x68<<1)//iako je adresa 7 bitna mora biti shiftana za jedno mjesto u lijevo
+#define WHO_AM_I_REG 0x75// vraća adresu 
+#define PWR_MGMT_1_REG 0x6B// podešavanje power mode i izvora sata
+#define CONFIG_REG 0x1A//konfiguracija
+#define GYRO_CONFIG_REG 0x1B//konfiguracija žiroskopa
+#define ACCELERO_CONFIG_REG 0x1C//konfiguracija akcelerometra
+#define GYRO_XOUT_H_REG 0x43
+#define ACCEL_XOUT_H_REG 0x3B
+#define TEMP_H_REG 0x41
+
+class Senzor{
+    public:
+    Senzor(PinName Sda, PinName Scl);
+    char testConnection ();
+    void setBW (char command);
+    void setPowerManagement (char command);
+    void GYRO_CONFIG(char command);
+    void ACCEL_CONFIG(char command);
+    void getAccel(float *accel);
+    void getGyro(float *gyro);
+    void getAccAngle(float *angle);
+    void getOffset(float *accOffset, float *gyroOffset, int sample);
+    void finalAngle (float *angle);
+    float getTemp();
+    
+    private:
+    I2C mpu;
+    
+    };
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Sat Nov 16 10:14:58 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/TextLCD/#308d188a2d3a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Nov 16 10:14:58 2019 +0000
@@ -0,0 +1,77 @@
+#include "mbed.h"
+#include "MPU.h"
+#include "HCSR04.h"
+#include "TextLCD.h"
+
+class LED_interrupt
+{
+
+    public:
+        LED_interrupt(PinName tipkalo_pin, PinName led_pin) : _stanje(tipkalo_pin), _tipkalo(led_pin)
+        {
+            _tipkalo.fall(callback(this, &LED_interrupt::promjena));
+            off();
+        };
+    
+        void off(void)
+        {
+            _stanje = 0;
+        };
+    
+        void promjena(void)
+        {
+            _stanje = !_stanje;
+        };
+    
+    private:
+        DigitalOut _stanje;
+        InterruptIn _tipkalo;
+};
+
+
+Serial pc(USBTX,USBRX);
+Senzor mpu(p9,p10);                                             // žiroskop (Sda, Scl)
+HCSR04 sensor(p7,p8);                                           // ultrazvučni senzor (trig, echo)
+TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2);    // display (rs, e, d4-d7)
+
+void calc()
+{
+    sensor.startMeasurement();
+}
+
+int main()
+{
+    float angle[3] = {0, 0, 0};
+    float temp;
+    float sampleTime = 0.5;
+    Ticker ticker;
+    float distance;
+
+    pc.printf("\nPocnimo\n");
+
+    sensor.setRanges(2, 400);
+    pc.printf("Minimum sensor range = %g cm\n\rMaximum sensor range = %g cm\n\r", sensor.getMinRange(), sensor.getMaxRange());
+    ticker.attach(&calc, sampleTime);
+
+    while(true) {
+
+        LED_interrupt(p15,p16);
+
+        mpu.finalAngle(angle);
+        temp=mpu.getTemp();
+        distance = sensor.getDistance_mm();
+
+        //lcd.cls();
+        //lcd.locate(0,1);
+        pc.printf("\npitch: %.4f", angle[0]);
+
+        //lcd.locate(0,0);
+        pc.printf("\nroll: %.4f", angle[1]);
+
+        pc.printf("\nTemp: %.4f",temp);
+        pc.printf("\nDistance: %f mm\r", distance);
+
+        wait(1);
+
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Nov 16 10:14:58 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file