File content as of revision 1:2173f1dbfe1c:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */
#include "mbed.h"
#include "VL53L0X.h"
#include "tb6612.h"
#include "QEI.h"

I2C         i2c(D14, D15);
VL53L0X     vl_sensors[2] = {(&i2c),(&i2c)}; //may need to use 2 not 3
BusOut      vl_shutdown(D0,D12); //D0 and D1 do not seem to work on the nucleo
Serial      usb(USBTX, USBRX, 115200);
TB6612      motorL(D2,D4,D5);
TB6612      motorR(D9,D10,D11);
QEI         ENC1(D7,D8,NC,3); //may need to be 12 not 3 Left motor
QEI         ENC2(D3,D6,NC,3); //may need to be 12 not 3 Right motor
Timer       Tim1;

int main()
{
    ENC1.reset(); //reset encoders
    ENC2.reset();

    const float motorGoal = 500; //declare motor rpm goal
    float e_speed_1 = 0; //starting error value
    float e_speed_sum_1 = 0; //starting error sum value
    float e_speed_pre_1 = 0; //starting error previous value
    float e_speed_2 = 0;
    float e_speed_sum_2 = 0;
    float e_speed_pre_2 = 0;
    float kp = 1.2; //proportional PID constant
    float ki = 0.4; //integral PID constant 
    float kd = 0.6; // derivative PID constant
    float pvSpeed1; //
    float pvSpeed2;
    float e_pwm_1 = 0; // initial pwm error 
    float e_pwm_2 = 0; 
    float e_pwm_sum_1 = 0; //initial pwm sum error
    float e_pwm_sum_2 = 0;
    float e_pwm_pre_1 = 0;  //initial error pwm previous
    float e_pwm_pre_2 = 0;
    float SpeedQ1 = 0.5; //initial Q1 value to be removed from starting rpm to begin error calcs
    float SpeedQ2 = 0.5;
    float PWMi1 = 0.2; //initial PWM value passed to motors to begin measurement
    float PWMi2 = 0.2;


    usb.printf("Multiple VL53L0X\n\n\r");

    uint8_t expander_shutdown_mask = 1;
    for(uint8_t i = 0; i < 2 ; i++) {
        vl_shutdown = expander_shutdown_mask;
        expander_shutdown_mask = (expander_shutdown_mask << 1) + 1;
        vl_sensors[i].init();
        vl_sensors[i].setDeviceAddress(0x40 + i);
        vl_sensors[i].setModeContinuous();
        vl_sensors[i].startContinuous();
    }
    uint16_t results[2]; //Sensor code to assign  I2C addresses to each sensor

    Tim1.start();
    Tim1.reset();
    float PrevTim = 0;
    motorL.setSpeed(PWMi1); //set motors to starting pwm sped
    motorR.setSpeed(PWMi2);

    while(1) {
        wait_ms(100);
        /*int EC1P = ENC1.getPulses();
        int EC2P = ENC2.getPulses();
        DigitalIn  button(BUTTON1);
        DigitalOut led(LED1);
        led = button;   // Equivalent to led.write(button.read())*/

        for(uint8_t i = 0; i < 2 ; i++) {
            results[i] = vl_sensors[i].getRangeMillimeters();
        } //sensor measurements 

        //usb.printf("1: %4imm 2: %4imm", results[0], results[1]);
        //usb.printf("1 Pulses: %d , 2 Pulses: %d\r\n", EC1P, EC2P);

        /***** motor speed correction***********/



        int EC1 = abs(ENC1.getPulses()); //reads encoder pulses
        int EC2 = abs(ENC2.getPulses());
        float Revm1 = EC1*10; //correct rpm 60pprev ec1=pulse per 0.1 sec, ec1*600=pulse per min; revpermin= pulsepermin/pulseperrev
        float Revm2 = EC2*10; //correct rpm
        usb.printf(" Rev1 = %f rpm, Rev2 = %f rpm \r\n", Revm1, Revm2);
       // usb.printf("1 Pulses: %d , 2 Pulses: %d\r\n", EC1, EC2);
        float Tchange = Tim1.read() - PrevTim; // calculates change in time since last reading
        PrevTim = Tim1.read(); //sets new time reading as previous
        ENC1.reset(); //resets encoder pulses
        ENC2.reset();

        /////////////////PID ///////////////////////
        
        //e_speed_1 = motorGoal - Revm1; //rpm error
        //e_speed_2 = motorGoal - Revm2;
        
        //e_pwm_1 = ((e_speed_1 * SpeedQ1)/Revm1); //converts rpm error to pwm error value
        //e_pwm_2 = ((e_speed_2 * SpeedQ2)/Revm2);
        
        //float SpeedN1 = ( ((e_pwm_1*kp + e_pwm_sum_1*ki + ((e_pwm_1 - e_pwm_pre_1)/Tchange)*kd)) + 0.5); //need offset value to vary around instead of 0 point
        //float SpeedN2 = ( ((e_pwm_2*kp + e_pwm_sum_2*ki + ((e_pwm_2 - e_pwm_pre_2)/Tchange)*kd)) + 0.5);
        //e_pwm_pre_1 = e_pwm_1; //sets previous speed to current
        //e_pwm_pre_2 = e_pwm_2;
        //e_pwm_sum_1 += (e_pwm_1 * Tchange); // adds all errors in speed together multiplied by their time period
        //e_pwm_sum_2 += (e_pwm_2 * Tchange);
        //usb.printf("SN1 = %f , SN2 = %f \r\n", SpeedN1, SpeedN2);
        //usb.printf("error1: %f , error2: %f \r\n", e_pwm_1, e_pwm_2);

        //SpeedQ1 = SpeedN1; // /10;
        //SpeedQ2 = SpeedN2; // /10;
        //usb.printf("SQ1 = %f , SQ2 = %f \r\n", SpeedQ1, SpeedQ2);
        


        /*if (SpeedQ1 > 10) {
            SpeedQ1 = 1;
        }
        if (SpeedQ2 > 10) {
            SpeedQ2 = 1;
        }
        if (SpeedQ1 < -10) {
            SpeedQ1 = -1;
        }
        if (SpeedQ2 < -10) {
            SpeedQ2 = -1;
        }


        if (e_pwm_sum_1 > 10) {
            e_pwm_sum_1 = 10;
        }
        if (e_pwm_sum_1 < -10) {
            e_pwm_sum_1 = -10;
        }
        if (e_pwm_sum_2 > 10) {
            e_pwm_sum_2 = 10;
        }
        if (e_pwm_sum_2 < -10) {
            e_pwm_sum_2 = -10;
        }
        usb.printf("SQ2 = %f , SQ1 = %f \r\n", SpeedQ1, SpeedQ2);
        PWMi1 = PWMi1 - SpeedQ1;
        PWMi2 = PWMi2 - SpeedQ2; 

        motorR.setSpeed(PWMi2);
        motorL.setSpeed(PWMi1); 
        usb.printf("PWMi1 = %f , PWMi2 = %f \r\n", PWMi1, PWMi2); 
        */

        //need to work out ki kp kd

        /*if (results[0] <= 200 && results[1]<=200){
            motorL.setSpeed(SpeedN1);
            motorR.setSpeed(SpeedN2);
            usb.printf("forward\n\r");
            }

        if (results[0] <= 200 && results[1] > 200) {
            motorL.setSpeed(SpeedN1);
            motorR.setSpeed(-SpeedN2);
            usb.printf("right turn\n\r");
            }

        if(results[0] > 200 && results[1] <= 200) {
            motorL.setSpeed(-SpeedN1);
            motorR.setSpeed(SpeedN2);
            usb.printf("left turn\n\r");
            }

        if(results[0] > 200 && results[1] > 200) {
            motorL.setSpeed(-SpeedN1);
            motorR.setSpeed(-SpeedN2);
            usb.printf("back\n\r");
            }*/
    }
}