S2Project final failed code
Dependencies: QEI TB6612 VL53L0X
Revision 1:2173f1dbfe1c, committed 2022-05-08
- Comitter:
- sas638
- Date:
- Sun May 08 20:04:11 2022 +0000
- Parent:
- 0:bcce413163a4
- Commit message:
- Project failed final;
Changed in this revision
diff -r bcce413163a4 -r 2173f1dbfe1c CONTRIBUTING.md --- a/CONTRIBUTING.md Thu Jan 27 22:46:00 2022 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,5 +0,0 @@ -# Contributing to Mbed OS - -Mbed OS is an open-source, device software platform for the Internet of Things. Contributions are an important part of the platform, and our goal is to make it as simple as possible to become a contributor. - -To encourage productive collaboration, as well as robust, consistent and maintainable code, we have a set of guidelines for [contributing to Mbed OS](https://os.mbed.com/docs/mbed-os/latest/contributing/index.html).
diff -r bcce413163a4 -r 2173f1dbfe1c QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Sun May 08 20:04:11 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sas638/code/QEI/#6f2c18570954
diff -r bcce413163a4 -r 2173f1dbfe1c README.md --- a/README.md Thu Jan 27 22:46:00 2022 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,64 +0,0 @@ -![](./resources/official_armmbed_example_badge.png) -# Blinky Mbed OS example - -The example project is part of the [Arm Mbed OS Official Examples](https://os.mbed.com/code/) and is the [getting started example for Mbed OS](https://os.mbed.com/docs/mbed-os/v5.14/quick-start/index.html). It contains an application that repeatedly blinks an LED on supported [Mbed boards](https://os.mbed.com/platforms/). - -You can build the project with all supported [Mbed OS build tools](https://os.mbed.com/docs/mbed-os/latest/tools/index.html). However, this example project specifically refers to the command-line interface tool [Arm Mbed CLI](https://github.com/ARMmbed/mbed-cli#installing-mbed-cli). -(Note: To see a rendered example you can import into the Arm Online Compiler, please see our [import quick start](https://os.mbed.com/docs/mbed-os/latest/quick-start/online-with-the-online-compiler.html#importing-the-code).) - -1. [Install Mbed CLI](https://os.mbed.com/docs/mbed-os/latest/quick-start/offline-with-mbed-cli.html). - -1. Clone this repository on your system, and change the current directory to where the project was cloned: - - ```bash - $ git clone git@github.com:armmbed/mbed-os-example-blinky && cd mbed-os-example-blinky - ``` - - Alternatively, you can download the example project with Arm Mbed CLI using the `import` subcommand: - - ```bash - $ mbed import mbed-os-example-blinky && cd mbed-os-example-blinky - ``` - - -## Application functionality - -The `main()` function is the single thread in the application. It toggles the state of a digital output connected to an LED on the board. - -## Building and running - -1. Connect a USB cable between the USB port on the board and the host computer. -2. <a name="build_cmd"></a> Run the following command to build the example project and program the microcontroller flash memory: - ```bash - $ mbed compile -m <TARGET> -t <TOOLCHAIN> --flash - ``` -The binary is located at `./BUILD/<TARGET>/<TOOLCHAIN>/mbed-os-example-blinky.bin`. - -Alternatively, you can manually copy the binary to the board, which you mount on the host computer over USB. - -Depending on the target, you can build the example project with the `GCC_ARM`, `ARM` or `IAR` toolchain. After installing Arm Mbed CLI, run the command below to determine which toolchain supports your target: - -```bash -$ mbed compile -S -``` - -## Expected output -The LED on your target turns on and off every 500 milliseconds. - - -## Troubleshooting -If you have problems, you can review the [documentation](https://os.mbed.com/docs/latest/tutorials/debugging.html) for suggestions on what could be wrong and how to fix it. - -## Related Links - -* [Mbed OS Stats API](https://os.mbed.com/docs/latest/apis/mbed-statistics.html). -* [Mbed OS Configuration](https://os.mbed.com/docs/latest/reference/configuration.html). -* [Mbed OS Serial Communication](https://os.mbed.com/docs/latest/tutorials/serial-communication.html). -* [Mbed OS bare metal](https://os.mbed.com/docs/mbed-os/latest/reference/mbed-os-bare-metal.html). -* [Mbed boards](https://os.mbed.com/platforms/). - -### License and contributions - -The software is provided under Apache-2.0 license. Contributions to this project are accepted under the same license. Please see contributing.md for more info. - -This project contains code from other projects. The original license text is included in those source files. They must comply with our license guide.
diff -r bcce413163a4 -r 2173f1dbfe1c main.cpp --- a/main.cpp Thu Jan 27 22:46:00 2022 +0000 +++ b/main.cpp Sun May 08 20:04:11 2022 +0000 @@ -5,19 +5,47 @@ #include "mbed.h" #include "VL53L0X.h" #include "tb6612.h" +#include "QEI.h" I2C i2c(D14, D15); -VL53L0X vl_sensors[2] = {(&i2c),(&i2c)}; -BusOut vl_shutdown(D11,D12); +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(D8,D9,D10); -TB6612 motorR(D3,D4,D5); +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() { - motorL.setSpeed(1.00); - motorR.setSpeed(1.00); - + 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; @@ -29,11 +57,126 @@ vl_sensors[i].setModeContinuous(); vl_sensors[i].startContinuous(); } - uint16_t results[2]; + 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; } - usb.printf("1: %4imm 2: %4imm\n\r", results[0], results[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"); + }*/ } } \ No newline at end of file