Lab Encoder plot theta

Lab Encoder/.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

Lab Encoder/.vscode/extensions.json

@@ -0,0 +1,10 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

Lab Encoder/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

Lab Encoder/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

Lab Encoder/platformio.ini

@@ -0,0 +1,15 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:uno]
+platform = atmelavr
+board = uno
+framework = arduino
+monitor_speed = 115200

Lab Encoder/src/main.cpp

@@ -0,0 +1,133 @@
+#include <Arduino.h>
+
+#define ENCA 2
+#define ENCB 3
+
+#define MOTORA 7
+#define MOTORB 8
+#define MOTOREN 9
+
+uint8_t state = 0;
+uint8_t lastState = 0;
+
+int32_t theta = 0;
+int32_t targetTheta = 100;
+
+int32_t error = 0;
+int32_t lastError = 0;
+int32_t dError = 0;
+int32_t iError = 0;
+int32_t lastInstant = 0;
+int32_t instant;
+float Kp = 10;
+float Kd = 0;
+float Ki = 0;
+
+bool f = false;
+
+int32_t period = 10000;
+
+void update() {
+  delay(1);
+  lastInstant = instant;
+  instant = micros();
+  int32_t deltaT = instant - lastInstant;
+  lastError = error;
+  error = targetTheta - theta;
+  dError = (error - lastError)/deltaT;
+  iError = iError + error*deltaT;
+  if (iError > 255)
+    iError = 255;
+  else if (iError < -255)
+    iError = -255;
+}
+
+int16_t pid() { return Kp * error + Ki * iError + Kd * dError; }
+
+void setPower(int16_t power) {
+  if (power > 255) {
+    power = 255;
+  } else if (power < -255) {
+    power = -255;
+  }
+
+  if (power > 0) {
+    analogWrite(MOTOREN, power);
+    digitalWrite(MOTORA, HIGH);
+    digitalWrite(MOTORB, LOW);
+  } else {
+    analogWrite(MOTOREN, -power);
+    digitalWrite(MOTORA, LOW);
+    digitalWrite(MOTORB, HIGH);
+  }
+}
+
+void plot(){
+  Serial.print(">timestamp:");
+  Serial.println(micros());
+  Serial.print(">theta:");
+  Serial.println(theta);
+  Serial.print(">targetTheta:");
+  Serial.println(targetTheta);
+}
+
+void encoderIsr() {
+  lastState = state;
+  state = (digitalRead(3) << 1) + digitalRead(2);
+  switch (lastState) {
+  case 0:
+    if (state == 1) {
+      theta++;
+    } else {
+      theta--;
+    }
+    break;
+  case 1:
+    if (state == 3) {
+      theta++;
+    } else {
+      theta--;
+    }
+    break;
+  case 3:
+    if (state == 2) {
+      theta++;
+    } else {
+      theta--;
+    }
+    break;
+  case 2:
+    if (state == 0) {
+      theta++;
+    } else {
+      theta--;
+    }
+    break;
+  }
+}
+
+void setup() {
+  Serial.begin(115200);
+  pinMode(LED_BUILTIN, OUTPUT);
+  pinMode(ENCA, INPUT);
+  pinMode(ENCB, INPUT);
+  pinMode(MOTORA, OUTPUT);
+  pinMode(MOTORB, OUTPUT);
+  pinMode(MOTOREN, OUTPUT);
+  attachInterrupt(digitalPinToInterrupt(ENCA), encoderIsr, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(ENCB), encoderIsr, CHANGE);
+}
+
+void loop() {
+  if (!f && millis() % period < period/2) {
+    targetTheta = 100;
+    f = true;
+  } else if (f && millis() % period > period/2) {
+    targetTheta = -100;
+    f = false;
+  }
+  plot();
+  update();
+  setPower(pid());
+  delay(10);
+}

Lab Encoder/test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html