sSubject line (try to keep under 50 characters)

Multi-line description of commit,
feel free to be detailed.

Signed-off-by: Abel Tim <git@abeltim.com>

3 files changed, 130 insertions, 36 deletions

diff --git a/code/debug/motors/with-pid/with-pid.ino b/code/debug/motors/with-pid/with-pid.ino
new file mode 100644
index 0000000..ce8e811
--- /dev/null
+++ b/code/debug/motors/with-pid/with-pid.ino
@@ -0,0 +1,99 @@
+//motor: https://www.pololu.com/product/4861
+//motor driver: https://www.pololu.com/product/2997
+
+//Create the timers
+IntervalTimer pwm_timer;
+IntervalTimer enc_timer;
+
+//define the pins
+//If the motors are spinning incorectly, change pins not the drive function.
+int led = 13;
+
+int m1enA = 6;
+int m1enB = 5;
+int m1pwm1 = 4;
+int m1pwm2 = 3;
+int m1encoA = 32;
+int m1encoB = 31;
+
+int m3encoB = 23;
+
+//PWM variables
+int pwmacc = 100; //how accurate the PWM is, means you can enter value from 0 to the value of pwmacc
+int PWMval1 = 0;  //value of the PWM1, means you can enter value from 0 to the value of pwmacc
+
+//encoder variables
+const int refreshrate = 50; //refresh rate of the encoder
+volatile long oneENCcount = 0; //the amount of pulses from motor one encoder
+
+double ONErpm = 0.00; //the amount of revolutions motor one makes per minute
+
+
+
+void setup() {
+  pinMode(m1enA, OUTPUT);
+  pinMode(m1enB, OUTPUT);
+  pinMode(m1pwm1, OUTPUT);
+  pinMode(m1pwm2, OUTPUT);
+
+  pwm_timer.begin(updatePWM, refreshrate); //speed at wich PWM is updated PWM
+  enc_timer.begin(enc_refr, 50);  //refresh rate of the encoder
+
+  attachInterrupt(digitalPinToInterrupt(m1encoA), AoneIRS, CHANGE); //calls void AoneIRS if there is a change of the stare of m1encoA
+  attachInterrupt(digitalPinToInterrupt(m1encoB), BoneIRS, CHANGE); //ditto
+
+  Serial.begin(9600);
+  Serial.print("start");
+
+}
+
+void updatePWM() {
+  //PWM one
+  //if the entered valvue is less the zero set it to zero, because lower that zero is not posible
+  if (PWMval1 < 0) {
+    PWMval1 = 0;
+  }
+  //if the enterd value is higher than the maximum posible value, set it to max.
+  if (PWMval1 > pwmacc) {
+    PWMval1 = pwmacc;
+  }
+  //check if the counter has passed the PWM value, and turn of the motor based on that.
+  if (counter1 < PWMval1) {
+    digitalWrite(m1enA, HIGH);
+  } else {
+    digitalWrite(m1enA, LOW);
+  }
+  counter1++;
+
+  if (counter1 >= pwmacc) {
+    counter1 = 0;
+  }
+}
+
+void AoneIRS(){
+  oneENCcount++ //adds one to the count
+}
+void BoneIRS(){
+  oneENCcount++ //dieselbe
+}
+
+void enc_refr(){
+  //calculate RPM. It gets updated 48 times per rotation.
+  ONErpm = (oneENCcount/48.00) * (60000000.00 / refreshrate)
+  //reset the counting to zero for next cycle.
+  oneENCcount = 0;
+}
+void PID(int speed){
+  Serial.println(ONErpm);
+  PWMval1 = ; //enter output from PID algorith here
+  digitalWrite(m1enB, LOW);
+  digitalWrite(m1pwm1, HIGH);
+  digitalWrite(m1pwm2, LOW);
+}
+
+//the loop routine runs over and over again forever:
+void loop() {
+  drive(50); //speed value between 0-100
+  Serial.println("Motor one is at: "+ ONErpm + " RPM.");
+  delay(50);
+}
diff --git a/code/main/teensy_player/prj.conf b/code/main/teensy_player/prj.conf
index 75bb3d2..8f5b5f3 100644
--- a/code/main/teensy_player/prj.conf
+++ b/code/main/teensy_player/prj.conf
@@ -1,2 +1,3 @@
 CONFIG_GPIO=y
 CONFIG_USB_DEVICE_INITIALIZE_AT_BOOT=y
+CONFIG_PRINTK=y
diff --git a/code/main/teensy_player/src/main.c b/code/main/teensy_player/src/main.c
index f8aff35..e082f66 100644
--- a/code/main/teensy_player/src/main.c
+++ b/code/main/teensy_player/src/main.c
@@ -1,48 +1,42 @@
-/*
- * Copyright (c) 2016 Intel Corporation
- *
- * SPDX-License-Identifier: Apache-2.0
- */
-
-#include <stdio.h>
 #include <zephyr/kernel.h>
+#include <zephyr/device.h>
 #include <zephyr/drivers/gpio.h>
+#include <zephyr/sys/printk.h>
+#include <zephyr/sys/__assert.h>
+#include <string.h>
 
-/* 1000 msec = 1 sec */
-#define SLEEP_TIME_MS   1000
+/* size of stack area used by each thread */
+#define STACKSIZE 1024
 
-/* The devicetree node identifier for the "led0" alias. */
-#define LED0_NODE DT_ALIAS(led0)
+/*the pinout configuration*/
+/*https://docs.zephyrproject.org/latest/boards/pjrc/teensy4/doc/index.html*/
+/*motors pins*/
+#define m1enA DT_ALIAS(GPIO2_10)    /*pin 6*/
+#define m1enB DT_ALIAS(GPIO4_8)     /*pin 5*/
+#define m1pwm1 DT_ALIAS(GPIO4_6)    /*pin 4*/
+#define m1pwm2 DT_ALIAS(GPIO4_5)    /*pin 3*/
+/*encoder pins*/
+#define m1encoA DT_ALIAS(GPIO2_12)  /*pin 32*//*encoder number 1*/
+#define Nm1encoA INT_TO_POINTER(0)    /*gives encoder number N*/
+#define m1encoB DT_ALIAS(GPIO3_22)  /*pin 31*//*encoder number 2*/
+#define Nm1encoB INT_TO_POINTER(1)    /*gives encoder number N*/
 
-/*
- * A build error on this line means your board is unsupported.
- * See the sample documentation for information on how to fix this.
- */
-static const struct gpio_dt_spec led = GPIO_DT_SPEC_GET(LED0_NODE, gpios);
 
-int main(void)
-{
-    int ret;
-    bool led_state = true;
+/*set priorities*/
+#define ENC_ISR_PRIO 1 /* set encoder interrupt service routine*/
 
-    if (!gpio_is_ready_dt(&led)) {
-        return 0;
-    }
 
-    ret = gpio_pin_configure_dt(&led, GPIO_OUTPUT_ACTIVE);
-    if (ret < 0) {
-        return 0;
+void ENCO_ISR(void *encN){
+    if (encN == 0){
+
     }
 
-    while (1) {
-        ret = gpio_pin_toggle_dt(&led);
-        if (ret < 0) {
-            return 0;
-        }
+}
 
-        led_state = !led_state;
-        printf("LED state: %s\n", led_state ? "ON" : "OFF");
-        k_msleep(SLEEP_TIME_MS);
-    }
-    return 0;
+
+void ENC_ISR_installer(void)
+{
+    /*sets up the interupts for the encoder at same priority, need to configure FIFO in case of simultanius call*/
+    IRQ_CONNECT(MY_DEV_IRQ, ENC_ISR_PRIO, m1encoA_ISR, Nm1encoA, MY_IRQ_FLAGS);
+    IRQ_CONNECT(MY_DEV_IRQ, ENC_ISR_PRIO, m1encoB_ISR, Nm1encoB, MY_IRQ_FLAGS);
 }