From 99620f78f825daa33d08dc07189a1af63acbe91d Mon Sep 17 00:00:00 2001 From: ErnestoELC Date: Tue, 17 Aug 2021 14:40:26 +0200 Subject: [PATCH 1/4] Correct Servo and PID examples --- .../PID_Position_test/PID_Position_test.ino | 119 +++++++++--------- examples/Nano/ServoTest/ServoTest.ino | 29 ++--- 2 files changed, 70 insertions(+), 78 deletions(-) diff --git a/examples/Nano/PID_Position_test/PID_Position_test.ino b/examples/Nano/PID_Position_test/PID_Position_test.ino index 3dc65f6..69d6cf3 100644 --- a/examples/Nano/PID_Position_test/PID_Position_test.ino +++ b/examples/Nano/PID_Position_test/PID_Position_test.ino @@ -1,5 +1,4 @@ -#include -//#include +#include #define INTERRUPT_PIN 6 //Variable to store the battery voltage @@ -9,6 +8,9 @@ static int batteryVoltage; static int duty = 0; int target; +float P; +float I; +float D; void setup() { @@ -16,15 +18,15 @@ void setup() Serial.begin(115200); while (!Serial); - //Establishing the communication with the Motor Carrier + //Establishing the communication with the motor shield if (controller.begin()) { - Serial.print("Motor Carrier connected, firmware version "); + Serial.print("MKR Motor Shield connected, firmware version "); Serial.println(controller.getFWVersion()); } else { - Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); + Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch"); while (1); } @@ -40,7 +42,7 @@ void setup() Serial.print("V, Raw "); Serial.println(battery.getRaw()); - int dutyInit = 0; // at 50 it works as expected, at 60 shift sides and is too small duty to move, at 70 is very big duty. + int dutyInit = 0; // at 50 it works as espected, at 60 shift sides and is too small duty to move, at 70 is very big duty. M1.setDuty(dutyInit); M2.setDuty(dutyInit); M3.setDuty(dutyInit); @@ -48,47 +50,51 @@ void setup() Serial.print("Duty: "); Serial.println(dutyInit); - /************* PID 1 ***********************/ - -// pid1.setControlMode(CL_POSITION); -// -// //pid1.resetGains(); -// //pid1.setLimits(-100,100); -// pid1.setGains(0.01f, 0.017f, 0.0f); //Proportional(change) Integral(change) Derivative -// Serial.print("P Gain: "); -// Serial.println((float)pid1.getPgain()); -// Serial.print("I Gain: "); -// Serial.println((float)pid1.getIgain()); -// Serial.print("D Gain: "); -// Serial.println((float)pid1.getDgain(), 7); -// Serial.println(""); -// -// encoder1.resetCounter(0); -// Serial.print("encoder1: "); -// Serial.println(encoder1.getRawCount()); -// target = 1000; -// pid1.setSetpoint(TARGET_POSITION, target); + P = 0.07f;//0.07 //0.2 + I = 0.0f; + D = 0.007f; - /************* PID 2 ***********************/ + /************* PID 1 ***********************/ - pid2.setControlMode(CL_POSITION); + pid1.setControlMode(CL_POSITION); //pid1.resetGains(); //pid1.setLimits(-100,100); - pid2.setGains(0.1f, 0.0f, 0.0f); //Proportional(change) Integral(change) Derivative + pid1.setGains(P, I, D); //Proportional(change) Integral(change) Derivative Serial.print("P Gain: "); - Serial.println((float)pid2.getPgain()); + Serial.println((float)pid1.getPgain()); Serial.print("I Gain: "); - Serial.println((float)pid2.getIgain()); + Serial.println((float)pid1.getIgain()); Serial.print("D Gain: "); - Serial.println((float)pid2.getDgain(), 7); + Serial.println((float)pid1.getDgain(), 7); Serial.println(""); - encoder2.resetCounter(0); - Serial.print("encoder2: "); - Serial.println(encoder2.getRawCount()); - target = 1000; - pid2.setSetpoint(TARGET_POSITION, target); + encoder1.resetCounter(0); + Serial.print("encoder1: "); + Serial.println(encoder1.getRawCount()); + target = 5000; + pid1.setSetpoint(TARGET_POSITION, target); + + /************* PID 2 ***********************/ + + // pid2.setControlMode(CL_POSITION); + // + // //pid1.resetGains(); + // //pid1.setLimits(-100,100); + // pid2.setGains(P, I, D); //Proportional(change) Integral(change) Derivative + // Serial.print("P Gain: "); + // Serial.println((float)pid2.getPgain()); + // Serial.print("I Gain: "); + // Serial.println((float)pid2.getIgain()); + // Serial.print("D Gain: "); + // Serial.println((float)pid2.getDgain(), 7); + // Serial.println(""); + // + // encoder2.resetCounter(0); + // Serial.print("encoder2: "); + // Serial.println(encoder2.getRawCount()); + // target = 1000; + // pid2.setSetpoint(TARGET_POSITION, target); } @@ -96,32 +102,29 @@ void loop() { /************* PID 1 ***********************/ -// Serial.print("encoder1: "); -// Serial.print(encoder1.getRawCount()); -// Serial.print(" target: "); -// Serial.println(target); -// if (encoder1.getRawCount() == target) { -// target += 1000; -// Serial.print("Target reached: Setting new target.."); -// pid1.setSetpoint(TARGET_POSITION, target); -// //delay(5000); -// } - - /************* PID 2 ***********************/ - - Serial.print("encoder2: "); - Serial.print(encoder2.getRawCount()); + Serial.print("encoder1: "); + Serial.print(encoder1.getRawCount()); Serial.print(" target: "); Serial.println(target); - if (encoder2.getRawCount() == target) { + if (encoder1.getRawCount() == target) { target += 1000; Serial.print("Target reached: Setting new target.."); - pid2.setSetpoint(TARGET_POSITION, target); + pid1.setSetpoint(TARGET_POSITION, target); //delay(5000); } - //--------------------------------------- - controller.ping(); - //wait + /************* PID 2 ***********************/ + + // Serial.print("encoder2: "); + // Serial.print(encoder2.getRawCount()); + // Serial.print(" target: "); + // Serial.println(target); + // if (encoder2.getRawCount() == target) { + // target += 1000; + // Serial.print("Target reached: Setting new target.."); + // pid2.setSetpoint(TARGET_POSITION, target); + // //delay(5000); + // } + delay(50); -} +} \ No newline at end of file diff --git a/examples/Nano/ServoTest/ServoTest.ino b/examples/Nano/ServoTest/ServoTest.ino index caf21fc..93db7af 100644 --- a/examples/Nano/ServoTest/ServoTest.ino +++ b/examples/Nano/ServoTest/ServoTest.ino @@ -1,7 +1,5 @@ -#include + #include -//#include -//#include #define INTERRUPT_PIN 6 @@ -11,15 +9,15 @@ void setup() Serial.begin(115200); //while (!Serial); - //Establishing the communication with the Motor Carrier + //Establishing the communication with the motor shield if (controller.begin()) { - Serial.print("Motor Carrier connected, firmware version "); + Serial.print("MKR Motor Shield connected, firmware version "); Serial.println(controller.getFWVersion()); } else { - Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); + Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch"); while (1); } @@ -32,10 +30,6 @@ void setup() M2.setDuty(0); M3.setDuty(0); M4.setDuty(0); - - if (!PMIC.enableBoostMode()) { - Serial.println("Error enabling Boost Mode"); - } } void loop() { @@ -43,14 +37,14 @@ void loop() { //Servo sweep from 0 position to 180 for (int i = 0; i < 180; i += 1) { - //Choose which of the servo connectors you want to use: servo1(default), servo2, servo3 or servo4 + //Choose what of all the servo connectors do you want to use: servo1(default), servo2, servo3 or servo4 servo1.setAngle(i); servo2.setAngle(i); servo3.setAngle(i); servo4.setAngle(i); Serial.print("Servos position"); Serial.println(i); - delay(20); + delay(100); } delay(200); @@ -58,20 +52,15 @@ void loop() { //Servo sweep from 180 position to 0 for (int i = 180; i > 0; i -= 1) { - //Choose which of the servo connectors you want to use: servo1(default), servo2, servo3 or servo4 + //Choose what of all the servo connectors do you want to use: servo1(default), servo2, servo3 or servo4 servo1.setAngle(i); servo2.setAngle(i); servo3.setAngle(i); servo4.setAngle(i); Serial.print("Servos position: "); Serial.println(i); - delay(20); + delay(100); } - - //Keep active the communication between MKR board & MKR Motor Carrier - //Ping the SAMD11 - controller.ping(); - //wait - delay(50); + delay(200); } From ee7c297909b7120ac59242f26c3e30f6393b1c22 Mon Sep 17 00:00:00 2001 From: ErnestoELC Date: Tue, 17 Aug 2021 14:57:27 +0200 Subject: [PATCH 2/4] correct typos in examples --- examples/Nano/PID_Position_test/PID_Position_test.ino | 8 ++++---- examples/Nano/ServoTest/ServoTest.ino | 10 +++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/Nano/PID_Position_test/PID_Position_test.ino b/examples/Nano/PID_Position_test/PID_Position_test.ino index 69d6cf3..5461128 100644 --- a/examples/Nano/PID_Position_test/PID_Position_test.ino +++ b/examples/Nano/PID_Position_test/PID_Position_test.ino @@ -18,15 +18,15 @@ void setup() Serial.begin(115200); while (!Serial); - //Establishing the communication with the motor shield + //Establishing the communication with the motor carrier if (controller.begin()) { - Serial.print("MKR Motor Shield connected, firmware version "); + Serial.print("MKR Motor Carrier connected, firmware version "); Serial.println(controller.getFWVersion()); } else { - Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch"); + Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); while (1); } @@ -42,7 +42,7 @@ void setup() Serial.print("V, Raw "); Serial.println(battery.getRaw()); - int dutyInit = 0; // at 50 it works as espected, at 60 shift sides and is too small duty to move, at 70 is very big duty. + int dutyInit = 0; // at 50 it works as expected, at 60 shift sides and is too small duty to move, at 70 is very big duty. M1.setDuty(dutyInit); M2.setDuty(dutyInit); M3.setDuty(dutyInit); diff --git a/examples/Nano/ServoTest/ServoTest.ino b/examples/Nano/ServoTest/ServoTest.ino index 93db7af..4cb5db0 100644 --- a/examples/Nano/ServoTest/ServoTest.ino +++ b/examples/Nano/ServoTest/ServoTest.ino @@ -9,15 +9,15 @@ void setup() Serial.begin(115200); //while (!Serial); - //Establishing the communication with the motor shield + //Establishing the communication with the motor Carrier if (controller.begin()) { - Serial.print("MKR Motor Shield connected, firmware version "); + Serial.print("MKR Motor Carrier connected, firmware version "); Serial.println(controller.getFWVersion()); } else { - Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch"); + Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); while (1); } @@ -37,7 +37,7 @@ void loop() { //Servo sweep from 0 position to 180 for (int i = 0; i < 180; i += 1) { - //Choose what of all the servo connectors do you want to use: servo1(default), servo2, servo3 or servo4 + //Set angle to each specific servo motor servo1.setAngle(i); servo2.setAngle(i); servo3.setAngle(i); @@ -52,7 +52,7 @@ void loop() { //Servo sweep from 180 position to 0 for (int i = 180; i > 0; i -= 1) { - //Choose what of all the servo connectors do you want to use: servo1(default), servo2, servo3 or servo4 + //Set angle to each specific servo motor servo1.setAngle(i); servo2.setAngle(i); servo3.setAngle(i); From 40759da1880b042819046ec923b69de94c5dd368 Mon Sep 17 00:00:00 2001 From: ErnestoELC Date: Tue, 17 Aug 2021 14:59:07 +0200 Subject: [PATCH 3/4] Remove MKR Motor carrier for just Motor carrier --- examples/Nano/PID_Position_test/PID_Position_test.ino | 2 +- examples/Nano/ServoTest/ServoTest.ino | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/Nano/PID_Position_test/PID_Position_test.ino b/examples/Nano/PID_Position_test/PID_Position_test.ino index 5461128..0de9fea 100644 --- a/examples/Nano/PID_Position_test/PID_Position_test.ino +++ b/examples/Nano/PID_Position_test/PID_Position_test.ino @@ -21,7 +21,7 @@ void setup() //Establishing the communication with the motor carrier if (controller.begin()) { - Serial.print("MKR Motor Carrier connected, firmware version "); + Serial.print("Motor Carrier connected, firmware version "); Serial.println(controller.getFWVersion()); } else diff --git a/examples/Nano/ServoTest/ServoTest.ino b/examples/Nano/ServoTest/ServoTest.ino index 4cb5db0..542d082 100644 --- a/examples/Nano/ServoTest/ServoTest.ino +++ b/examples/Nano/ServoTest/ServoTest.ino @@ -12,7 +12,7 @@ void setup() //Establishing the communication with the motor Carrier if (controller.begin()) { - Serial.print("MKR Motor Carrier connected, firmware version "); + Serial.print("Motor Carrier connected, firmware version "); Serial.println(controller.getFWVersion()); } else From d6122427484f78e9441314b7254788170236e6c1 Mon Sep 17 00:00:00 2001 From: ErnestoELC Date: Tue, 17 Aug 2021 15:00:23 +0200 Subject: [PATCH 4/4] typos --- examples/Nano/PID_Position_test/PID_Position_test.ino | 2 +- examples/Nano/ServoTest/ServoTest.ino | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/Nano/PID_Position_test/PID_Position_test.ino b/examples/Nano/PID_Position_test/PID_Position_test.ino index 0de9fea..2c86aaa 100644 --- a/examples/Nano/PID_Position_test/PID_Position_test.ino +++ b/examples/Nano/PID_Position_test/PID_Position_test.ino @@ -18,7 +18,7 @@ void setup() Serial.begin(115200); while (!Serial); - //Establishing the communication with the motor carrier + //Establishing the communication with the Motor Carrier if (controller.begin()) { Serial.print("Motor Carrier connected, firmware version "); diff --git a/examples/Nano/ServoTest/ServoTest.ino b/examples/Nano/ServoTest/ServoTest.ino index 542d082..88e0ec2 100644 --- a/examples/Nano/ServoTest/ServoTest.ino +++ b/examples/Nano/ServoTest/ServoTest.ino @@ -9,7 +9,7 @@ void setup() Serial.begin(115200); //while (!Serial); - //Establishing the communication with the motor Carrier + //Establishing the communication with the Motor Carrier if (controller.begin()) { Serial.print("Motor Carrier connected, firmware version ");