Sam Stansfield
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
--- 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).
--- /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
--- 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 @@ - -# 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.
--- 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