diff options
Diffstat (limited to 'CODE/debug/motordriver')
23 files changed, 3015 insertions, 0 deletions
diff --git a/CODE/debug/motordriver/PID/PID.ino b/CODE/debug/motordriver/PID/PID.ino new file mode 100644 index 0000000..c7f351a --- /dev/null +++ b/CODE/debug/motordriver/PID/PID.ino @@ -0,0 +1,112 @@ +#include <TimerThree.h> +#include <QuickPID.h> + + +//https://www.pololu.com/product/2997 +//https://www.pololu.com/product/4861 + + +float Setpoint, Input, Output; +float Kp = 2, Ki = 5, Kd = 1; +QuickPID myPID(&Input, &Output, &Setpoint); + + +int enA = 1; // PWM pin +int enB = 2; // GND +int pwm1 = 3; // Motor control pin 1 +int pwm2 = 4; // Motor control pin 2 +int encoA = 5; // Encoder pin A +int encoB = 6; // Encoder pin B + +double sp = 0.00; +double rpm = 0.00; +volatile long encount = 0; +unsigned long Ltime = 0; +unsigned long Ctime = 0; +unsigned long Ptime = 0; + +volatile int PWMval = 0; // PWM value 0-1000 + +void setup() { + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + + pinMode(encoA, INPUT_PULLUP); + pinMode(encoB, INPUT_PULLUP); + + attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE); + + Timer3.initialize(800); // 800 microseconds or 1.25kHz + Timer3.attachInterrupt(updatePWM); + + Serial.begin(9600); + + // PID settings + Input = rpm; + Setpoint = 500; + myPID.SetTunings(Kp, Ki, Kd); + myPID.SetMode(myPID.Control::automatic); +} + +void updatePWM() { + static int counter = 0; + + if (counter < PWMval) { + digitalWrite(enA, HIGH); + } else { + digitalWrite(enA, LOW); + } + + counter++; + if (counter >= 1000) { + counter = 0; + } +} + +void mcpsoftpwm(int value) { + if (value < 0) value = 0; + if (value > 1000) value = 1000; + + PWMval = value; +} + +void encoderISR() { + encount++; +} + +void readenc() { + Ctime = micros(); + Ptime = Ctime - Ltime; + Ltime = Ctime; + rpm = (encount / 211.2) * (60000000.0 / Ptime); // Adjust the encoder counts per revolution here if necessary + encount = 0; +} + +void resetenc() { + Ctime = micros(); + Ltime = Ctime; + encount = 0; +} + +void move(int sped) { + sp = abs(sped) * 0.1; + mcpsoftpwm(sp); + digitalWrite(enB, LOW); + if (sped < 0) { + digitalWrite(pwm1, LOW); + digitalWrite(pwm2, HIGH); + } else { + digitalWrite(pwm1, HIGH); + digitalWrite(pwm2, LOW); + } +} + +void loop() { + Input = rpm; + myPID.Compute(); + move(Output); + +} diff --git a/CODE/debug/motordriver/PID_autotune/PID.py b/CODE/debug/motordriver/PID_autotune/PID.py new file mode 100644 index 0000000..e24dbd6 --- /dev/null +++ b/CODE/debug/motordriver/PID_autotune/PID.py @@ -0,0 +1,131 @@ +import serial +import time +from simple_pid import PID +import matplotlib.pyplot as plt +from matplotlib.widgets import Slider, Button + +# Initialize serial communication +arduino = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=.1) + +# Function to write to and read from Arduino +def write_read(x): + arduino.write(bytes(x, 'utf-8')) + time.sleep(0.05) + data = arduino.readline().decode().strip() + return data + +# Real-time plotting setup +fig, (ax1, ax2) = plt.subplots(2, 1) +feedback_list = [] +control_list = [] +setpoint_list = [] + +def clamp(feedback, min_value, max_value): + return max(min(feedback, max_value), min_value) + +def update_plot(feedback, control, setpoint): + feedback_clamped = clamp(feedback, 0, 1000) # Clamp feedback value between 0 and 1000 + feedback_list.append(feedback_clamped) + control_list.append(control) + setpoint_list.append(setpoint) + + # Limit lists to 50 items for smoother plotting + if len(feedback_list) > 50: + feedback_list.pop(0) + control_list.pop(0) + setpoint_list.pop(0) + + ax1.clear() + ax2.clear() + + ax1.plot(feedback_list, label="Feedback") + ax1.plot(setpoint_list, label="Setpoint") + ax1.legend() + + ax2.plot(control_list, label="Control") + ax2.legend() + + ax1.set_ylabel('RPM') + ax2.set_ylabel('Control Signal') + ax2.set_xlabel('Time') + + plt.tight_layout() + plt.pause(0.05) # Pause to allow plot to update + +# Initialize PID controller with output limits +initial_setpoint = 500 # Initial setpoint +pid = PID(0, 0, 0, output_limits=(0, 1000)) +pid.setpoint = initial_setpoint + +# Initialize PID parameters (preserved across resets) +preserved_parameters = { + 'Kp': 0, + 'Ki': 0, + 'Kd': 0, + 'setpoint': initial_setpoint +} + +# Function to update PID parameters +def update_pid(val): + pid.Kp = s_kp.val + pid.Ki = s_ki.val + pid.Kd = s_kd.val + preserved_parameters['Kp'] = s_kp.val + preserved_parameters['Ki'] = s_ki.val + preserved_parameters['Kd'] = s_kd.val + setpoint = s_setpoint.val + pid.setpoint = setpoint + preserved_parameters['setpoint'] = setpoint + print(f"Setpoint: {setpoint}, Kp: {pid.Kp}, Ki: {pid.Ki}, Kd: {pid.Kd}") + +# Create sliders for PID parameters and setpoint +axcolor = 'lightgoldenrodyellow' +ax_setpoint = plt.axes([0.15, 0.1, 0.65, 0.03], facecolor=axcolor) +ax_kp = plt.axes([0.15, 0.15, 0.65, 0.03], facecolor=axcolor) +ax_ki = plt.axes([0.15, 0.2, 0.65, 0.03], facecolor=axcolor) +ax_kd = plt.axes([0.15, 0.25, 0.65, 0.03], facecolor=axcolor) + +s_setpoint = Slider(ax_setpoint, 'Setpoint', 0, 1000, valinit=initial_setpoint) +s_kp = Slider(ax_kp, 'Kp', 0, 10, valinit=preserved_parameters['Kp']) +s_ki = Slider(ax_ki, 'Ki', 0, 10, valinit=preserved_parameters['Ki']) +s_kd = Slider(ax_kd, 'Kd', 0, 10, valinit=preserved_parameters['Kd']) + +s_setpoint.on_changed(update_pid) +s_kp.on_changed(update_pid) +s_ki.on_changed(update_pid) +s_kd.on_changed(update_pid) + +# Function to reset algorithm and set speed to zero +def reset_algorithm(event): + global feedback_list, control_list, setpoint_list + feedback_list.clear() + control_list.clear() + setpoint_list.clear() + pid.reset() + s_setpoint.set_val(preserved_parameters['setpoint']) + s_kp.set_val(preserved_parameters['Kp']) + s_ki.set_val(preserved_parameters['Ki']) + s_kd.set_val(preserved_parameters['Kd']) + print("Algorithm reset") + time.sleep(1) # Delay of 1 second after reset + +# Create reset button +ax_reset = plt.axes([0.8, 0.02, 0.1, 0.05]) +btn_reset = Button(ax_reset, 'Reset') +btn_reset.on_clicked(reset_algorithm) + +# Function to update Arduino with clamped control signal +def update_arduino_control(control): + control_clamped = clamp(control, 0, 1000) + write_read(str(control_clamped)) + +# Control loop +while True: + feedback = float(write_read(str(pid.setpoint))) + feedback_clamped = clamp(feedback, 0, 1000) # Clamp feedback value between 0 and 1000 + control = pid(feedback_clamped) + update_arduino_control(control) + update_plot(feedback_clamped, control, pid.setpoint) + +# Keep plot open +plt.show() diff --git a/CODE/debug/motordriver/PID_autotune/PID_autotune.ino b/CODE/debug/motordriver/PID_autotune/PID_autotune.ino new file mode 100644 index 0000000..ab99c50 --- /dev/null +++ b/CODE/debug/motordriver/PID_autotune/PID_autotune.ino @@ -0,0 +1,82 @@ +int enA = 1; +int enB = 2; +int pwm1 = 3; +int pwm2 = 4; +int encoA = 5; +int encoB = 6; + +int speed = 1; // speed 0-1000 + +double sp = 0.00; +double rpm = 0.00; +volatile long encount = 0; +unsigned long Ltime = 0; +unsigned long Ctime = 0; +unsigned long Ptime = 0; + +int mess; + +void setup() { + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + + pinMode(encoA, INPUT_PULLUP); + pinMode(encoB, INPUT_PULLUP); + + attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE); + + Serial.begin(115200); + Serial.setTimeout(1); +} + +void encoderISR() { + encount++; +} + +void readenc() { + Ctime = micros(); + Ptime = Ctime - Ltime; + Ltime = Ctime; + rpm = (encount / 211.2) * (60000000.0 / Ptime); + /*Serial.print("RPM: "); + Serial.print(rpm); + Serial.print(" Sample period:"); + Serial.print(Ptime); + Serial.print("ms, encoder count:"); + Serial.println(encount); + */ + + encount = 0; +} +void resetenc(){ + Ctime = micros(); + Ltime = Ctime; + encount = 0; +} + +void move(int sped) { + sp = abs(sped)*2.55; + analogWrite(enA, sp); + digitalWrite(enB, LOW); + if (sped < 0) { + //Serial.print("back "); + digitalWrite(pwm1, LOW); + digitalWrite(pwm2, HIGH); + } else { + //Serial.print("forward "); + digitalWrite(pwm1, HIGH); + digitalWrite(pwm2, LOW); + } + //Serial.println(sp); +} + +void loop() { + while (!Serial.available()); + mess = Serial.readString().toInt(); + move(mess); + readenc(); + Serial.print(rpm); +} diff --git a/CODE/debug/motordriver/PID_autotune/man_test.py b/CODE/debug/motordriver/PID_autotune/man_test.py new file mode 100644 index 0000000..52c84e3 --- /dev/null +++ b/CODE/debug/motordriver/PID_autotune/man_test.py @@ -0,0 +1,16 @@ +import serial +import time + +arduino = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=.1) + +def write_read(x): + arduino.write(bytes(x, 'utf-8')) + time.sleep(0.05) + data = arduino.readline() + return data + +while True: + num = input("Enter a number: ") # Taking input from user + value = write_read(num) + print(value) # printing the value + diff --git a/CODE/debug/motordriver/arduino-softpwm-master.zip b/CODE/debug/motordriver/arduino-softpwm-master.zip Binary files differnew file mode 100644 index 0000000..dc713b7 --- /dev/null +++ b/CODE/debug/motordriver/arduino-softpwm-master.zip diff --git a/CODE/debug/motordriver/arduino-softpwm-master/.gitignore b/CODE/debug/motordriver/arduino-softpwm-master/.gitignore new file mode 100644 index 0000000..1605aab --- /dev/null +++ b/CODE/debug/motordriver/arduino-softpwm-master/.gitignore @@ -0,0 +1 @@ +SoftPWM.h.gch
\ No newline at end of file diff --git a/CODE/debug/motordriver/arduino-softpwm-master/README.md b/CODE/debug/motordriver/arduino-softpwm-master/README.md new file mode 100644 index 0000000..46e5456 --- /dev/null +++ b/CODE/debug/motordriver/arduino-softpwm-master/README.md @@ -0,0 +1,75 @@ +arduino-softpwm +=============== + +Software PWM library for [Arduino](https://arduino.cc/), and other compatible AVR boards. + +AVR microcontrollers provide hardware PWM on some pins but if you need PWM on other pins then it must be implemented in software. This library provides easy and efficient software PWM on any pin. Each channel can be set to a different PWM duty cycle. + +This library is licensed under [BSD3](https://opensource.org/licenses/BSD-3-Clause) + +#### Installation +- Download the most recent version here: https://github.com/Palatis/arduino-softpwm/archive/master.zip +- Using Arduino IDE 1.0.x: + - Sketch > Import Library... > Add Library... > select the downloaded file > Open +- Using Arduino IDE 1.5+: + - Sketch > Include Library > Add ZIP Library... > select the downloaded file > Open + + +#### Usage +See **File > Examples > arduino-softpwm > SoftPWM_example** for demonstration of library usage. + +Everything in this library except for preprocessor defines are wrapped in `namespace Palatis {}`, so if your compiler blames you about symbol not found, try add `Palatis::` before that thing. For example, `Palatis::SoftPWM.size()`. + +Alternatively, you can add `using namespace Palatis;` at the top of the source file to import everything under the `Palatis` namespace, this might be convenient if you're using my other libraries. + +`#define SOFTPWM_OUTPUT_DELAY` - Add this line above `#include <SoftPWM.h>` for a 1 PWM clock cycle delay between outputs to prevent large in-rush currents. + +`SOFTPWM_DEFINE_CHANNEL(CHANNEL, PMODE, PORT, BIT)` - Configure a pin for software PWM use. Consult the datasheet for your microcontroller for the appropriate `PORT` and `BIT` values for the physical pin. This information is shown in the pinout diagram, for example: [ATmega328P datasheet](http://www.atmel.com/Images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet_Summary.pdf) **Figure 1-1** found on page 3. If you want to determine the Arduino pin assigned to the physical pin http://www.pighixxx.com/test/pinoutspg/boards/ provides this information for the most popular Arduino boards or you can look at the pins_arduino.h file in the **variant** folder used by your board. +- Parameter: **CHANNEL** - The channel number is used as an ID for the pin. +- Parameter: **PMODE** - DDRx register of the pin's port. Append the port letter to `DDR`. For example: The [Arduino UNO diagram](http://www.pighixxx.com/test/portfolio-items/uno/?portfolioID=314) shows that Arduino pin 13 is **PB5** which means that the port is **B** so you should use the value `DDRB` for that pin. +- Parameter: **PORT** - The port of the pin. For example: Arduino pin 13 is **PB5** so you should use the value `PORTB` for that pin. +- Parameter: **BIT** - The bit of the pin. For example: Arduino pin 13 is **PB5** so you should use the value `PORTB5` for that pin. + +`SOFTPWM_DEFINE_CHANNEL_INVERT( CHANNEL, PMODE, PORT, BIT )` - Depending on your application you may prefer to invert the output. See `SOFTPWM_DEFINE_CHANNEL()` for description of parameters. + +`SOFTPWM_DEFINE_OBJECT(CHANNEL_CNT)` - Define the softPWM object with the default 256 PWM levels. +- Parameter: **CHANNEL_CNT** - The number of channels that are defined. + +`SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS)` - Define the softPWM object with the specified number of PWM levels. +- Parameter: **CHANNEL_CNT** - The number of channels that are defined. +- Parameter: **PWM_LEVELS** - The number of PWM levels. Using less PWM levels may allow a higher PWM frequency. The maximum value is 256. + +`SOFTPWM_DEFINE_EXTERN_OBJECT(CHANNEL_CNT)` - Add this if you want to use the SoftPWM object outside where it's defined. See `SOFTPWM_DEFINE_OBJECT()` for description of the parameter. + +`SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS)` - Add this if you want to use the SoftPWM object outside where it's defined. See `SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS()` for description of parameters. + +`SoftPWM.begin(hertz)` - Initialize SoftPWM. +- Parameter: **hertz** - The PWM frequency. Setting the value too high will cause incorrect operation, too low will cause a visible flicker. + - Type: long + +`SoftPWM.printInterruptLoad()` - Prints diagnostic information to the serial monitor. This can be used to find the optimal PWM frequency by setting different PWM frequency values in begin() and then checking the resulting interrupt load. Calling this function will momentarily turn off the PWM on all channels. + +`SoftPWM.set(channel_idx, value)` - Set the PWM level of the given channel. +- Parameter: **channel_idx** - The channel to set. + - Type: int +- Parameter: **value** - The PWM level to set. + - Type: byte + +`SoftPWM.size()` +- Returns: Number of channels defined. + - Type: size_t + +`SoftPWM.PWMlevels()` +- Returns: The number of PWM levels. + - Type: unsigned int + +`SoftPWM.allOff()` - Set the PWM value of all channels to 0. + + +#### Troubleshooting +- LEDs have a visible flicker, especially noticeable when the LED is moving relative to the viewer. + - The PWM frequency set in `SoftPWM.begin()` is too low. Use `SoftPWM.printInterruptLoad()` to determine the optimum PWM frequency. You may be able to achieve a higher PWM frequency by setting less PWM levels with `SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS()` or `SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS()`. +- Erratic PWM operation + - The interrupt load is too high. Use `SoftPWM.printInterruptLoad()` to determine the interrupt load. You can decrease the interrupt load by setting less PWM levels with `SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS()` or `SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS()` or setting the PWM frequency lower in `SoftPWM.begin()`. +- LED brightness changes between low brightness PWM values are larger than at brighter PWM values. + - This is caused by the way LEDs work and is not caused by a problem with the library. If possible, use more PWM levels or never allow the LED to get dimmer than the level below which the difference between PWM levels is too distinct. diff --git a/CODE/debug/motordriver/arduino-softpwm-master/examples/SoftPWM_example/SoftPWM_example.ino b/CODE/debug/motordriver/arduino-softpwm-master/examples/SoftPWM_example/SoftPWM_example.ino new file mode 100644 index 0000000..96fc0b2 --- /dev/null +++ b/CODE/debug/motordriver/arduino-softpwm-master/examples/SoftPWM_example/SoftPWM_example.ino @@ -0,0 +1,141 @@ +/* + Arduino-SoftPWM: a software PWM library for Arduino + Copyright 2016, Victor Tseng <palatis@gmail.com> + + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + 3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS + IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include <SoftPWM.h> + +/* pins_arduino.h defines the pin-port/bit mapping as PROGMEM so + you have to read them with pgm_read_xxx(). That's generally okay + for ordinary use, but really bad when you're writing super fast + codes because the compiler doesn't treat them as constants and + cannot optimize them away with sbi/cbi instructions. + + Therefore we have to tell the compiler the PORT and BIT here. + Hope someday we can find a way to workaround this. + + Check the manual of your MCU for port/bit mapping. + + The following example demonstrates setting channels for all pins + on the ATmega328P or ATmega168 used on Arduino Uno, Pro Mini, + Nano and other boards. */ +SOFTPWM_DEFINE_CHANNEL(0, DDRD, PORTD, PORTD0); //Arduino pin 0 +SOFTPWM_DEFINE_CHANNEL(1, DDRD, PORTD, PORTD1); //Arduino pin 1 +SOFTPWM_DEFINE_CHANNEL(2, DDRD, PORTD, PORTD2); //Arduino pin 2 +SOFTPWM_DEFINE_CHANNEL(3, DDRD, PORTD, PORTD3); //Arduino pin 3 +SOFTPWM_DEFINE_CHANNEL(4, DDRD, PORTD, PORTD4); //Arduino pin 4 +SOFTPWM_DEFINE_CHANNEL(5, DDRD, PORTD, PORTD5); //Arduino pin 5 +SOFTPWM_DEFINE_CHANNEL(6, DDRD, PORTD, PORTD6); //Arduino pin 6 +SOFTPWM_DEFINE_CHANNEL(7, DDRD, PORTD, PORTD7); //Arduino pin 7 +SOFTPWM_DEFINE_CHANNEL(8, DDRB, PORTB, PORTB0); //Arduino pin 8 +SOFTPWM_DEFINE_CHANNEL(9, DDRB, PORTB, PORTB1); //Arduino pin 9 +SOFTPWM_DEFINE_CHANNEL(10, DDRB, PORTB, PORTB2); //Arduino pin 10 +SOFTPWM_DEFINE_CHANNEL(11, DDRB, PORTB, PORTB3); //Arduino pin 11 +SOFTPWM_DEFINE_CHANNEL(12, DDRB, PORTB, PORTB4); //Arduino pin 12 +SOFTPWM_DEFINE_CHANNEL(13, DDRB, PORTB, PORTB5); //Arduino pin 13 +SOFTPWM_DEFINE_CHANNEL(14, DDRC, PORTC, PORTC0); //Arduino pin A0 +SOFTPWM_DEFINE_CHANNEL(15, DDRC, PORTC, PORTC1); //Arduino pin A1 +SOFTPWM_DEFINE_CHANNEL(16, DDRC, PORTC, PORTC2); //Arduino pin A2 +SOFTPWM_DEFINE_CHANNEL(17, DDRC, PORTC, PORTC3); //Arduino pin A3 +SOFTPWM_DEFINE_CHANNEL(18, DDRC, PORTC, PORTC4); //Arduino pin A4 +SOFTPWM_DEFINE_CHANNEL(19, DDRC, PORTC, PORTC5); //Arduino pin A5 + + +/* Or you may want inverted outputs: */ +/* + SOFTPWM_DEFINE_CHANNEL_INVERT(0, DDRD, PORTD, PORTD0); //Arduino pin 0 + SOFTPWM_DEFINE_CHANNEL_INVERT(1, DDRD, PORTD, PORTD1); //Arduino pin 1 + SOFTPWM_DEFINE_CHANNEL_INVERT(2, DDRD, PORTD, PORTD2); //Arduino pin 2 + SOFTPWM_DEFINE_CHANNEL_INVERT(3, DDRD, PORTD, PORTD3); //Arduino pin 3 + SOFTPWM_DEFINE_CHANNEL_INVERT(4, DDRD, PORTD, PORTD4); //Arduino pin 4 + SOFTPWM_DEFINE_CHANNEL_INVERT(5, DDRD, PORTD, PORTD5); //Arduino pin 5 + SOFTPWM_DEFINE_CHANNEL_INVERT(6, DDRD, PORTD, PORTD6); //Arduino pin 6 + SOFTPWM_DEFINE_CHANNEL_INVERT(7, DDRD, PORTD, PORTD7); //Arduino pin 7 + SOFTPWM_DEFINE_CHANNEL_INVERT(8, DDRB, PORTB, PORTB0); //Arduino pin 8 + SOFTPWM_DEFINE_CHANNEL_INVERT(9, DDRB, PORTB, PORTB2); //Arduino pin 9 + SOFTPWM_DEFINE_CHANNEL_INVERT(10, DDRB, PORTB, PORTB2); //Arduino pin 10 + SOFTPWM_DEFINE_CHANNEL_INVERT(11, DDRB, PORTB, PORTB3); //Arduino pin 11 + SOFTPWM_DEFINE_CHANNEL_INVERT(12, DDRB, PORTB, PORTB4); //Arduino pin 12 + SOFTPWM_DEFINE_CHANNEL_INVERT(13, DDRB, PORTB, PORTB5); //Arduino pin 13 + SOFTPWM_DEFINE_CHANNEL_INVERT(14, DDRC, PORTC, PORTC0); //Arduino pin A0 + SOFTPWM_DEFINE_CHANNEL_INVERT(15, DDRC, PORTC, PORTC1); //Arduino pin A1 + SOFTPWM_DEFINE_CHANNEL_INVERT(16, DDRC, PORTC, PORTC2); //Arduino pin A2 + SOFTPWM_DEFINE_CHANNEL_INVERT(17, DDRC, PORTC, PORTC3); //Arduino pin A3 + SOFTPWM_DEFINE_CHANNEL_INVERT(18, DDRC, PORTC, PORTC4); //Arduino pin A4 + SOFTPWM_DEFINE_CHANNEL_INVERT(19, DDRC, PORTC, PORTC5); //Arduino pin A5 +*/ + +/* Here you make an instance of desired channel counts you want + with the default 256 PWM levels (0 ~ 255). */ +//SOFTPWM_DEFINE_OBJECT(20); + +/* Or you can make one with only 100 PWM levels (0 ~ 99). + By using less PWM levels, you may be able to use higher + pwm frequencies. */ +SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(20, 100); + +/* If you want to use the SoftPWM object outside where it's defined, + add the following line to the file. */ +//SOFTPWM_DEFINE_EXTERN_OBJECT(16); +SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS(20, 100); + +void setup() { + Serial.begin(19200); + + // begin with 60hz pwm frequency + Palatis::SoftPWM.begin(60); + + // print interrupt load for diagnostic purposes + Palatis::SoftPWM.printInterruptLoad(); +} + +static volatile uint8_t v = 0; +void loop() { + for (uint8_t i = 0; i < Palatis::SoftPWM.size(); ++i) { + Serial.print(micros()); + Serial.print(" loop(): "); + Serial.println(i); + + unsigned long const WAIT = 1000000 / Palatis::SoftPWM.PWMlevels() / 2; + unsigned long nextMicros = 0; + for (int v = 0; v < Palatis::SoftPWM.PWMlevels() - 1; ++v) { + while (micros() < nextMicros); + nextMicros = micros() + WAIT; + Palatis::SoftPWM.set(i, v); + } + for (int v = Palatis::SoftPWM.PWMlevels() - 1; v >= 0; --v) { + while (micros() < nextMicros); + nextMicros = micros() + WAIT; + Palatis::SoftPWM.set(i, v); + } + } +} diff --git a/CODE/debug/motordriver/arduino-softpwm-master/keywords.txt b/CODE/debug/motordriver/arduino-softpwm-master/keywords.txt new file mode 100644 index 0000000..61d42b7 --- /dev/null +++ b/CODE/debug/motordriver/arduino-softpwm-master/keywords.txt @@ -0,0 +1,30 @@ +####################################### +# Syntax Coloring Map SoftPWM +####################################### +####################################### +# Datatypes (KEYWORD1) +####################################### +Palatis KEYWORD1 +SoftPWM KEYWORD1 +####################################### +# Methods and Functions (KEYWORD2) +####################################### +begin KEYWORD2 +set KEYWORD2 +size KEYWORD2 +PWMlevels KEYWORD2 +allOff KEYWORD2 +update KEYWORD2 +printInterruptLoad KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### +SOFTPWM_DEFINE_PINMODE LITERAL1 +SOFTPWM_DEFINE_CHANNEL LITERAL1 +SOFTPWM_DEFINE_CHANNEL_INVERT LITERAL1 +SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS LITERAL1 +SOFTPWM_DEFINE_OBJECT LITERAL1 +SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS LITERAL1 +SOFTPWM_DEFINE_EXTERN_OBJECT LITERAL1 +SOFTPWM_OUTPUT_DELAY LITERAL1 diff --git a/CODE/debug/motordriver/arduino-softpwm-master/library.properties b/CODE/debug/motordriver/arduino-softpwm-master/library.properties new file mode 100644 index 0000000..174c19b --- /dev/null +++ b/CODE/debug/motordriver/arduino-softpwm-master/library.properties @@ -0,0 +1,9 @@ +name=arduino-softpwm +version=1.0.1 +author=Victor Tseng <palatis@gmail.com> +maintainer=Victor Tseng <palatis@gmail.com> +sentence=Software PWM library for Arduino. +paragraph=Provides PWM on any pin. +category=Signal Input/Output +url=https://github.com/Palatis/arduino-softpwm +architectures=avr diff --git a/CODE/debug/motordriver/arduino-softpwm-master/src/SoftPWM.h b/CODE/debug/motordriver/arduino-softpwm-master/src/SoftPWM.h new file mode 100644 index 0000000..c315a4e --- /dev/null +++ b/CODE/debug/motordriver/arduino-softpwm-master/src/SoftPWM.h @@ -0,0 +1,255 @@ +/* + Arduino-SoftPWM: a software PWM library for Arduino + Copyright 2016, Victor Tseng <palatis@gmail.com> + + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + 3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS + IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef _SOFTPWM_H_ +#define _SOFTPWM_H_ + +#include <Arduino.h> + +// helper macros +#define SOFTPWM_DEFINE_PINMODE(CHANNEL, PMODE, PORT, BIT) \ + namespace Palatis { \ + template <> \ + inline void pinModeStatic<CHANNEL>(uint8_t const mode) { \ + if (mode == INPUT) { \ + bitClear(PMODE, BIT); bitClear(PORT, BIT); \ + } \ + else if (mode == INPUT_PULLUP) { \ + bitClear(PMODE, BIT); bitSet(PORT, BIT); \ + } \ + else { \ + bitSet(PMODE, BIT); \ + } \ + } \ + } + +#define SOFTPWM_DEFINE_CHANNEL(CHANNEL, PMODE, PORT, BIT) \ + namespace Palatis { \ + template <> \ + inline void bitWriteStatic<CHANNEL>(bool const value) { \ + bitWrite( PORT, BIT, value ); \ + } \ + } \ + SOFTPWM_DEFINE_PINMODE(CHANNEL, PMODE, PORT, BIT) + +#define SOFTPWM_DEFINE_CHANNEL_INVERT(CHANNEL, PMODE, PORT, BIT) \ + namespace Palatis { \ + template <> \ + inline void bitWriteStatic<CHANNEL>(bool const value) { \ + bitWrite(PORT, BIT, !value); \ + } \ + } \ + SOFTPWM_DEFINE_PINMODE(CHANNEL, PMODE, PORT, BIT) + +#if defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) +#define SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS) \ + namespace Palatis { \ + CSoftPWM<CHANNEL_CNT, PWM_LEVELS> SoftPWM; \ + } \ + ISR(TIM1_COMPA_vect) { \ + interrupts(); \ + Palatis::SoftPWM.update(); \ + } +#else + #if defined (__AVR_ATmega8__) || defined (__AVR_ATmega8A__) + #define TIMSK1 TIMSK + #endif +#define SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS) \ + namespace Palatis { \ + CSoftPWM<CHANNEL_CNT, PWM_LEVELS> SoftPWM; \ + } \ + ISR(TIMER1_COMPA_vect) { \ + interrupts(); \ + Palatis::SoftPWM.update(); \ + } +#endif + +#define SOFTPWM_DEFINE_OBJECT(CHANNEL_CNT) \ + SOFTPWM_DEFINE_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, 0) + +#define SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, PWM_LEVELS) \ + namespace Palatis { \ + extern CSoftPWM<CHANNEL_CNT, PWM_LEVELS> SoftPWM; \ + } + +#define SOFTPWM_DEFINE_EXTERN_OBJECT(CHANNEL_CNT) \ + SOFTPWM_DEFINE_EXTERN_OBJECT_WITH_PWM_LEVELS(CHANNEL_CNT, 0) + +// here comes the magic @o@ +namespace Palatis { + +template <int channel> inline void bitWriteStatic(bool value) {} +template <int channel> inline void pinModeStatic(uint8_t mode) {} + +template <int channel> +struct bitWriteStaticExpander { + void operator() (bool value) const { + bitWriteStatic<channel>(value); + bitWriteStaticExpander < channel - 1 > ()(value); + } + + void operator() (uint8_t const &count, uint8_t const * const &channels) const { +#ifdef SOFTPWM_OUTPUT_DELAY + bitWriteStatic<channel>((count + channel) < channels[channel]); +#else + bitWriteStatic<channel>(count < channels[channel]); +#endif + bitWriteStaticExpander < channel - 1 > ()(count, channels); + } +}; + +template <> +struct bitWriteStaticExpander < -1 > { + void operator() (bool) const {} + void operator() (uint8_t const &, uint8_t const * const &) const {} +}; + +template <int channel> +struct pinModeStaticExpander { + void operator() (uint8_t const mode) const + { + pinModeStatic<channel>(mode); + pinModeStaticExpander < channel - 1 > ()(mode); + } +}; + +template <> +struct pinModeStaticExpander < -1 > { + void operator() (uint8_t const mode) const {} +}; + +template <unsigned int num_channels, unsigned int num_PWM_levels> +class CSoftPWM { + public: + void begin(const unsigned long hertz) { + allOff(); //this prevents inverted channels from momentarily going LOW + asm volatile ("/************ pinModeStaticExpander begin ************/"); + const uint8_t oldSREG = SREG; + noInterrupts(); + pinModeStaticExpander < num_channels - 1 > ()( OUTPUT ); + SREG = oldSREG; + asm volatile ("/************ pinModeStaticExpander end ************/"); + + /* the setup of timer1 is stolen from ShiftPWM :-P + http://www.elcojacobs.com/shiftpwm/ */ + asm volatile ("/************ timer setup begin ************/"); + TCCR1A = 0b00000000; + TCCR1B = 0b00001001; + OCR1A = (F_CPU - hertz * PWMlevels() / 2) / (hertz * PWMlevels()); + bitSet(TIMSK1, OCIE1A); + asm volatile ("/************ timer setup end ************/"); + + _count = 0; + } + + void set(const int channel_idx, const uint8_t value) { + _channels[channel_idx] = value; + } + + size_t size() const { + return num_channels; + } + + unsigned int PWMlevels() const { + return num_PWM_levels ? num_PWM_levels : 256; + } + + void allOff() { + asm volatile ("/********** CSoftPWM::allOff() begin **********/"); + const uint8_t oldSREG = SREG; + noInterrupts(); + for (int i = 0; i < num_channels; ++i) + _channels[i] = 0; + bitWriteStaticExpander < num_channels - 1 > ()(false); + SREG = oldSREG; + asm volatile ("/********** CSoftPWM::allOff() end **********/"); + } + + /* This function cannot be private because the ISR uses it, and I have + no idea about how to make friends with ISR. :-( */ + void update() __attribute__((always_inline)) { + asm volatile ("/********** CSoftPWM::update() begin **********/"); + const uint8_t count = _count; + bitWriteStaticExpander < num_channels - 1 > ()(count, _channels); + ++_count; + if (_count == PWMlevels()) + _count = 0; + asm volatile ("/********** CSoftPWM::update() end **********/"); + } + + #if defined(HAVE_HWSERIAL0) + /* this function is stolen from ShiftPWM :-P + http://www.elcojacobs.com/shiftpwm/ */ + void printInterruptLoad() { + unsigned long time1, time2; + + bitSet(TIMSK1, OCIE1A); // enable interrupt + time1 = micros(); + delayMicroseconds(5000); + time1 = micros() - time1; + + bitClear(TIMSK1, OCIE1A); // disable interrupt + time2 = micros(); + delayMicroseconds(5000); + time2 = micros() - time2; + + const double load = static_cast<double>(time1 - time2) / time1; + const double interrupt_frequency = static_cast<double>(F_CPU) / (OCR1A + 1); + const double cycles_per_interrupt = load * F_CPU / interrupt_frequency; + + Serial.println(F("SoftPWM::printInterruptLoad():")); + Serial.print(F(" Load of interrupt: ")); + Serial.println(load, 10); + Serial.print(F(" Clock cycles per interrupt: ")); + Serial.println( cycles_per_interrupt ); + Serial.print(F(" Interrupt frequency: ")); + Serial.print(interrupt_frequency); + Serial.println(F(" Hz")); + Serial.print(F(" PWM frequency: ")); + Serial.print(interrupt_frequency / PWMlevels()); + Serial.println(F(" Hz")); + Serial.print(F(" PWM levels: ")); + Serial.println(PWMlevels()); + + bitSet(TIMSK1, OCIE1A); // enable interrupt again + } + #endif + + private: + uint8_t _channels[num_channels]; + uint8_t _count; +}; + +} // namespace Palatis +#endif // #ifndef _SOFTPWM_H_ diff --git a/CODE/debug/motordriver/calibrated_motor/calibrated_motor.ino b/CODE/debug/motordriver/calibrated_motor/calibrated_motor.ino new file mode 100644 index 0000000..60e2c04 --- /dev/null +++ b/CODE/debug/motordriver/calibrated_motor/calibrated_motor.ino @@ -0,0 +1,155 @@ +#include <TimerThree.h> + +#define ArrayElementCount(needRPM) (sizeof(needRPM) / sizeof(needRPM[0])) + +int enA = 1; //PWM pin +int enB = 2; //GND +int pwm1 = 3; // +int pwm2 = 4; +int encoA = 5; +int encoB = 6; + +const int maxspeed = 1000; + +double sp = 0.00; +double rpm = 0.00; +volatile long encount = 0; +unsigned long Ltime = 0; +unsigned long Ctime = 0; +unsigned long Ptime = 0; + +volatile int PWMval = 0; // PWM value 0-100 + +int motordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910}; +int needRPM = 41; + + +void setup() { + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + + + pinMode(encoA, INPUT_PULLUP); + pinMode(encoB, INPUT_PULLUP); + + attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE); + + Timer3.initialize(800); // 800 microseconds or 1.25kHz + Timer3.attachInterrupt(updatePWM); + + Serial.begin(9600); +} + +void updatePWM() { + static int counter = 0; + + if (counter < PWMval) { + digitalWrite(enA, HIGH); + } else { + digitalWrite(enA, LOW); + } + + counter++; + if (counter >= 100) { + counter = 0; + } +} + +void mcpsoftpwm(int value) { + if (value < 0) value = 0; + if (value > 100) value = 100; + + //Serial.print("softPWM pin: enA, value: "); + //Serial.println(value); + PWMval = value; +} + +void encoderISR() { + encount++; +} + +void readenc() { + Ctime = micros(); + Ptime = Ctime - Ltime; + Ltime = Ctime; + rpm = (encount / 211.2) * (60000000.0 / Ptime); + //Serial.print("RPM: "); + //Serial.print(rpm); + //Serial.print(" Sample period:"); + //Serial.print(Ptime); + //Serial.print("ms, encoder count:"); + //Serial.println(encount); + + encount = 0; +} +void resetenc(){ + Ctime = micros(); + Ltime = Ctime; + encount = 0; +} + +void move(int sped) { + sp = abs(sped)*0.1; + mcpsoftpwm(sp); + digitalWrite(enB, LOW); + if (sped < 0) { + //Serial.print("back "); + digitalWrite(pwm1, LOW); + digitalWrite(pwm2, HIGH); + } else { + //Serial.print("forward "); + digitalWrite(pwm1, HIGH); + digitalWrite(pwm2, LOW); + } + //Serial.println(sp); +} +void RPMmove(int Rsped) { + int idx = nearest(Rsped, motordata, ArrayElementCount(motordata)); + move(idx); +} + + +void loop() { + for (int i = 0; i < maxspeed; i++) { + Serial.print(motordata[i]); + Serial.print(", "); + } + Serial.println(""); + int idx = nearest(needRPM, motordata, ArrayElementCount(motordata)); + Serial.print(needRPM); + Serial.print(", "); + Serial.print(idx); + Serial.print(", "); + Serial.println(motordata[idx]); + delay(2500); + move(idx); + delay(1000); + resetenc(); + delay(100); + readenc(); + Serial.println(rpm); + delay(10); + + + +} +int nearest(int x, int myArray[], int elements) +{ + //https://forum.arduino.cc/t/search-for-nearest-number-in-an-array-help/245843/5 + int idx = 0; // by default near first element + + int distance = abs(myArray[idx] - x); + for (int i = 1; i < elements; i++) + { + int d = abs(myArray[i] - x); + if (d < distance) + { + idx = i; + distance = d; + } + } + return idx; +} diff --git a/CODE/debug/motordriver/kiwidrive_calc.svg b/CODE/debug/motordriver/kiwidrive_calc.svg new file mode 100644 index 0000000..3ca09e2 --- /dev/null +++ b/CODE/debug/motordriver/kiwidrive_calc.svg @@ -0,0 +1,323 @@ +<?xml version="1.0" encoding="UTF-8" standalone="no"?> +<svg + xmlns:dc="http://purl.org/dc/elements/1.1/" + xmlns:cc="http://creativecommons.org/ns#" + xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#" + xmlns:svg="http://www.w3.org/2000/svg" + xmlns="http://www.w3.org/2000/svg" + xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" + xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" + width="210mm" + height="297mm" + viewBox="0 0 210 297" + version="1.1" + id="svg8" + inkscape:version="1.0.2 (e86c870879, 2021-01-15)" + sodipodi:docname="kiwidrive_calc.svg"> + <defs + id="defs2" /> + <sodipodi:namedview + id="base" + pagecolor="#ffffff" + bordercolor="#666666" + borderopacity="1.0" + inkscape:pageopacity="0.0" + inkscape:pageshadow="2" + inkscape:zoom="1.4" + inkscape:cx="350.96101" + inkscape:cy="562.71867" + inkscape:document-units="mm" + inkscape:current-layer="layer1" + inkscape:document-rotation="0" + showgrid="false" + inkscape:window-width="1920" + inkscape:window-height="1125" + inkscape:window-x="2560" + inkscape:window-y="0" + inkscape:window-maximized="1" /> + <metadata + id="metadata5"> + <rdf:RDF> + <cc:Work + rdf:about=""> + <dc:format>image/svg+xml</dc:format> + <dc:type + rdf:resource="http://purl.org/dc/dcmitype/StillImage" /> + <dc:title></dc:title> + </cc:Work> + </rdf:RDF> + </metadata> + <g + inkscape:label="Layer 1" + inkscape:groupmode="layer" + id="layer1"> + <ellipse + style="fill:#ff0000;fill-rule:evenodd;stroke-width:0.264583" + id="path10" + cx="99.888367" + cy="134.68913" + rx="39.564999" + ry="38.587662" /> + <path + style="fill:#a0a0a0;stroke:none;stroke-width:0.0944941" + d="m 139.72661,146.01799 c -1.63175,-1.14292 -2.20174,-11.72757 -0.90654,-16.83422 0.39799,-1.56917 0.89479,-2.32023 1.34986,-2.04073 0.45996,0.2825 0.99183,1.8708 1.27726,3.81422 0.51922,3.53519 0.43362,9.37384 -0.18105,12.34962 -0.43334,2.09796 -0.99722,3.09095 -1.53953,2.71111 z" + id="path22" /> + <path + style="fill:#a0a0a0;stroke:none;stroke-width:0.0944941" + d="m 63.718331,112.73542 c -0.31847,-1.96659 6.871434,-9.75535 11.435089,-12.38756 1.402322,-0.808823 2.287091,-0.976383 2.404045,-0.455293 0.1182,0.526683 -0.64948,2.015393 -1.843555,3.575053 -2.17208,2.83712 -6.417053,6.84677 -8.978746,8.48104 -1.806017,1.15218 -2.910982,1.44035 -3.016833,0.78676 z" + id="path22-3" /> + <path + style="fill:#a0a0a0;stroke:none;stroke-width:0.0944941" + d="m 74.709966,169.73071 c -1.970594,0.29267 -9.664519,-6.99861 -12.236726,-11.59635 -0.790382,-1.4128 -0.946345,-2.29969 -0.423758,-2.4098 0.528179,-0.11131 2.0067,0.67581 3.550588,1.89022 2.808431,2.20905 6.762138,6.50618 8.362712,9.08905 1.12843,1.82096 1.402097,2.9296 0.747184,3.02688 z" + id="path22-3-6" + inkscape:transform-center-x="37.420481" + inkscape:transform-center-y="-21.837511" /> + <rect + style="fill:#0000ff;fill-rule:evenodd;stroke-width:0.264583" + id="rect64" + width="186.92551" + height="1.8174417" + x="17.333752" + y="135.38728" /> + <rect + style="fill:#0000ff;fill-rule:evenodd;stroke-width:0.264583" + id="rect64-7" + width="186.92551" + height="1.8174417" + x="31.72876" + y="-100.25121" + transform="rotate(89.560511)" /> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="197.80376" + y="132.93736" + id="text88"><tspan + sodipodi:role="line" + id="tspan86" + x="197.80376" + y="132.93736" + style="stroke-width:0.264583">x</tspan></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="101.31552" + y="42.965397" + id="text92"><tspan + sodipodi:role="line" + id="tspan90" + x="101.31552" + y="42.965397" + style="stroke-width:0.264583">y</tspan></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="92.677986" + y="145.29591" + id="text96"><tspan + sodipodi:role="line" + id="tspan94" + x="92.677986" + y="145.29591" + style="stroke-width:0.264583">0</tspan></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="142.35989" + y="149.98514" + id="text96-5"><tspan + sodipodi:role="line" + id="tspan94-3" + x="142.35989" + y="149.98514" + style="stroke-width:0.264583">1</tspan></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="94.360558" + y="101.32148" + id="text96-5-5"><tspan + sodipodi:role="line" + id="tspan94-3-6" + x="94.360558" + y="101.32148" + style="stroke-width:0.264583">1</tspan></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="46.688869" + y="147.24464" + id="text96-5-5-2"><tspan + sodipodi:role="line" + x="46.688869" + y="147.24464" + style="stroke-width:0.264583" + id="tspan170">-1</tspan></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="88.356491" + y="186.57739" + id="text96-5-5-2-1"><tspan + sodipodi:role="line" + x="88.356491" + y="186.57739" + style="stroke-width:0.264583" + id="tspan170-2">-1</tspan></text> + <circle + id="path198" + style="fill:#000000;stroke:#000000;stroke-width:0.264583" + cx="144.59926" + cy="132.06937" + r="0.012488274" /> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="140.65088" + y="140.43587" + id="text202"><tspan + sodipodi:role="line" + id="tspan200" + x="140.65088" + y="140.43587" + style="font-size:4.23333px;stroke-width:0.264583">0rad</tspan><tspan + sodipodi:role="line" + x="140.65088" + y="153.66498" + style="stroke-width:0.264583" + id="tspan204" /></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:4.23333px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="59.606441" + y="102.25119" + id="text208"><tspan + sodipodi:role="line" + id="tspan206" + x="59.606441" + y="102.25119" + style="font-size:4.23333px;stroke-width:0.264583">2.09rad</tspan></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:4.23333px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="53.299278" + y="167.29311" + id="text212"><tspan + sodipodi:role="line" + id="tspan210" + x="53.299278" + y="167.29311" + style="font-size:4.23333px;stroke-width:0.264583">4.18rad</tspan></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="142.78784" + y="135.75781" + id="text216"><tspan + sodipodi:role="line" + id="tspan214" + x="142.78784" + y="135.75781" + style="stroke-width:0.264583">A</tspan><tspan + sodipodi:role="line" + x="142.78784" + y="148.98694" + style="stroke-width:0.264583" + id="tspan218" /><tspan + sodipodi:role="line" + x="142.78784" + y="162.21606" + style="stroke-width:0.264583" + id="tspan220" /></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="68.655945" + y="112.41339" + id="text216-7"><tspan + sodipodi:role="line" + id="tspan214-0" + x="68.655945" + y="112.41339" + style="stroke-width:0.264583">B</tspan><tspan + sodipodi:role="line" + x="68.655945" + y="125.64252" + style="stroke-width:0.264583" + id="tspan218-9" /><tspan + sodipodi:role="line" + x="68.655945" + y="138.87164" + style="stroke-width:0.264583" + id="tspan220-3" /></text> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="67.528885" + y="164.8815" + id="text216-7-6"><tspan + sodipodi:role="line" + id="tspan214-0-0" + x="67.528885" + y="164.8815" + style="stroke-width:0.264583">C</tspan><tspan + sodipodi:role="line" + x="67.528885" + y="178.11063" + style="stroke-width:0.264583" + id="tspan298" /><tspan + sodipodi:role="line" + x="67.528885" + y="191.33975" + style="stroke-width:0.264583" + id="tspan218-9-6" /><tspan + sodipodi:role="line" + x="67.528885" + y="204.56888" + style="stroke-width:0.264583" + id="tspan220-3-2" /></text> + <rect + style="fill:#c900ff;fill-opacity:1;fill-rule:evenodd;stroke-width:0.264583" + id="rect318" + width="40.081783" + height="3.5191765" + x="-9.3766651" + y="166.49748" + transform="rotate(-39.971721)" /> + <rect + style="fill:#c900ff;fill-opacity:1;fill-rule:evenodd;stroke-width:0.12328" + id="rect318-6" + width="27.441587" + height="1.1159221" + x="104.34172" + y="135.77676" + transform="matrix(0.99998555,0.0053753,-0.01704694,0.99985469,0,0)" /> + <rect + style="fill:#c900ff;fill-opacity:1;fill-rule:evenodd;stroke-width:0.127056" + id="rect318-6-1" + width="29.147621" + height="1.1159564" + x="-135.70338" + y="128.97763" + transform="matrix(-0.00379001,-0.99999282,0.9999488,0.01011907,0,0)" /> + <text + xml:space="preserve" + style="font-style:normal;font-weight:normal;font-size:4.58611px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583" + x="102.17614" + y="135.32759" + id="text208-8"><tspan + sodipodi:role="line" + id="tspan206-7" + x="102.17614" + y="135.32759" + style="font-size:4.58611px;stroke-width:0.264583">0.52rad</tspan></text> + <rect + style="fill:#c98b00;fill-opacity:1;fill-rule:evenodd;stroke-width:0.121786" + id="rect318-6-1-0" + width="26.779816" + height="1.1159475" + x="-135.58972" + y="138.72464" + transform="matrix(-0.00412511,-0.99999149,0.99995678,0.00929711,0,0)" /> + </g> +</svg> diff --git a/CODE/debug/motordriver/motor_calibration/example_outp/.avrage_motor_values.dat.kate-swp b/CODE/debug/motordriver/motor_calibration/example_outp/.avrage_motor_values.dat.kate-swp Binary files differnew file mode 100644 index 0000000..3b62227 --- /dev/null +++ b/CODE/debug/motordriver/motor_calibration/example_outp/.avrage_motor_values.dat.kate-swp diff --git a/CODE/debug/motordriver/motor_calibration/example_outp/avrage_motor_values.dat b/CODE/debug/motordriver/motor_calibration/example_outp/avrage_motor_values.dat new file mode 100644 index 0000000..dbbfee0 --- /dev/null +++ b/CODE/debug/motordriver/motor_calibration/example_outp/avrage_motor_values.dat @@ -0,0 +1 @@ +0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 256, 257, 258, 259, 260, 261, 262, 263, 264, 265, 266, 267, 268, 269, 270, 271, 272, 273, 274, 275, 276, 277, 278, 279, 280, 281, 282, 283, 284, 285, 286, 287, 288, 289, 290, 291, 292, 293, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303, 304, 305, 306, 307, 308, 309, 310, 311, 312, 313, 314, 315, 316, 317, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, 328, 329, 330, 331, 332, 333, 334, 335, 336, 337, 338, 339, 340, 341, 342, 343, 344, 345, 346, 347, 348, 349, 350, 351, 352, 353, 354, 355, 356, 357, 358, 359, 360, 361, 362, 363, 364, 365, 366, 367, 368, 369, 370, 371, 372, 373, 374, 375, 376, 377, 378, 379, 380, 381, 382, 383, 384, 385, 386, 387, 388, 389, 390, 391, 392, 393, 394, 395, 396, 397, 398, 399, 400, 401, 402, 403, 404, 405, 406, 407, 408, 409, 410, 411, 412, 413, 414, 415, 416, 417, 418, 419, 420, 421, 422, 423, 424, 425, 426, 427, 428, 429, 430, 431, 432, 433, 434, 435, 436, 437, 438, 439, 440, 441, 442, 443, 444, 445, 446, 447, 448, 449, 450, 451, 452, 453, 454, 455, 456, 457, 458, 459, 460, 461, 462, 463, 464, 465, 466, 467, 468, 469, 470, 471, 472, 473, 474, 475, 476, 477, 478, 479, 480, 481, 482, 483, 484, 485, 486, 487, 488, 489, 490, 491, 492, 493, 494, 495, 496, 497, 498, 499, 500, 501, 502, 503, 504, 505, 506, 507, 508, 509, 510, 511, 512, 513, 514, 515, 516, 517, 518, 519, 520, 521, 522, 523, 524, 525, 526, 527, 528, 529, 530, 531, 532, 533, 534, 535, 536, 537, 538, 539, 540, 541, 542, 543, 544, 545, 546, 547, 548, 549, 550, 551, 552, 553, 554, 555, 556, 557, 558, 559, 560, 561, 562, 563, 564, 565, 566, 567, 568, 569, 570, 571, 572, 573, 574, 575, 576, 577, 578, 579, 580, 581, 582, 583, 584, 585, 586, 587, 588, 589, 590, 591, 592, 593, 594, 595, 596, 597, 598, 599, 600, 601, 602, 603, 604, 605, 606, 607, 608, 609, 610, 611, 612, 613, 614, 615, 616, 617, 618, 619, 620, 621, 622, 623, 624, 625, 626, 627, 628, 629, 630, 631, 632, 633, 634, 635, 636, 637, 638, 639, 640, 641, 642, 643, 644, 645, 646, 647, 648, 649, 650, 651, 652, 653, 654, 655, 656, 657, 658, 659, 660, 661, 662, 663, 664, 665, 666, 667, 668, 669, 670, 671, 672, 673, 674, 675, 676, 677, 678, 679, 680, 681, 682, 683, 684, 685, 686, 687, 688, 689, 690, 691, 692, 693, 694, 695, 696, 697, 698, 699, 700, 701, 702, 703, 704, 705, 706, 707, 708, 709, 710, 711, 712, 713, 714, 715, 716, 717, 718, 719, 720, 721, 722, 723, 724, 725, 726, 727, 728, 729, 730, 731, 732, 733, 734, 735, 736, 737, 738, 739, 740, 741, 742, 743, 744, 745, 746, 747, 748, 749, 750, 751, 752, 753, 754, 755, 756, 757, 758, 759, 760, 761, 762, 763, 764, 765, 766, 767, 768, 769, 770, 771, 772, 773, 774, 775, 776, 777, 778, 779, 780, 781, 782, 783, 784, 785, 786, 787, 788, 789, 790, 791, 792, 793, 794, 795, 796, 797, 798, 799, 800, 801, 802, 803, 804, 805, 806, 807, 808, 809, 810, 811, 812, 813, 814, 815, 816, 817, 818, 819, 820, 821, 822, 823, 824, 825, 826, 827, 828, 829, 830, 831, 832, 833, 834, 835, 836, 837, 838, 839, 840, 841, 842, 843, 844, 845, 846, 847, 848, 849, 850, 851, 852, 853, 854, 855, 856, 857, 858, 859, 860, 861, 862, 863, 864, 865, 866, 867, 868, 869, 870, 871, 872, 873, 874, 875, 876, 877, 878, 879, 880, 881, 882, 883, 884, 885, 886, 887, 888, 889, 890, 891, 892, 893, 894, 895, 896, 897, 898, 899, 900, 901, 902, 903, 904, 905, 906, 907, 908, 909, 910, 911, 912, 913, 914, 915, 916, 917, 918, 919, 920, 921, 922, 923, 924, 925, 926, 927, 928, 929, 930, 931, 932, 933, 934, 935, 936, 937, 938, 939, 940, 941, 942, 943, 944, 945, 946, 947, 948, 949, 950, 951, 952, 953, 954, 955, 956, 957, 958, 959, 960, 961, 962, 963, 964, 965, 966, 967, 968, 969, 970, 971, 972, 973, 974, 975, 976, 977, 978, 979, 980, 981, 982, 983, 984, 985, 986, 987, 988, 989, 990, 991, 992, 993, 994, 995, 996, 997, 998, 999, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910,
\ No newline at end of file diff --git a/CODE/debug/motordriver/motor_calibration/motor_calibration.ino b/CODE/debug/motordriver/motor_calibration/motor_calibration.ino new file mode 100644 index 0000000..362d130 --- /dev/null +++ b/CODE/debug/motordriver/motor_calibration/motor_calibration.ino @@ -0,0 +1,231 @@ +#include <TimerThree.h> +#include <SD.h> +#include <SPI.h> + +int enA = 1; //PWM pin +int enB = 2; //GND +int pwm1 = 3; // +int pwm2 = 4; +int encoA = 5; +int encoB = 6; + +const int maxspeed = 1000; + +double sp = 0.00; +double rpm = 0.00; +volatile long encount = 0; +unsigned long Ltime = 0; +unsigned long Ctime = 0; +unsigned long Ptime = 0; + +volatile int PWMval = 0; // PWM value 0-100 +File ; +File avrage; +Sd2Card card; +SdVolume volume; +SdFile root; +const int chipSelect = BUILTIN_SDCARD; +int x = 1; + +int Nloop = 0; +int totloopdata[maxspeed] = {}; + + +void setup() { + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + + pinMode(encoA, INPUT_PULLUP); + pinMode(encoB, INPUT_PULLUP); + + attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE); + + Timer3.initialize(800); // 800 microseconds or 1.25kHz + Timer3.attachInterrupt(updatePWM); + + Serial.begin(9600); +} + +void updatePWM() { + static int counter = 0; + + if (counter < PWMval) { + digitalWrite(enA, HIGH); + } else { + digitalWrite(enA, LOW); + } + + counter++; + if (counter >= 100) { + counter = 0; + } +} +void mcpsoftpwm(int value) { + if (value < 0) value = 0; + if (value > 100) value = 100; + + //Serial.print("softPWM pin: enA, value: "); + //Serial.println(value); + PWMval = value; +} + +void encoderISR() { + encount++; +} + +void readenc() { + Ctime = micros(); + Ptime = Ctime - Ltime; + Ltime = Ctime; + rpm = (encount / 211.2) * (60000000.0 / Ptime); + //Serial.print("RPM: "); + //Serial.print(rpm); + //Serial.print(" Sample period:"); + //Serial.print(Ptime); + //Serial.print("ms, encoder count:"); + //Serial.println(encount); + + encount = 0; +} +void resetenc(){ + Ctime = micros(); + Ltime = Ctime; + encount = 0; +} + +void move(int sped) { + sp = abs(sped)*0.1; + mcpsoftpwm(sp); + digitalWrite(enB, LOW); + if (sped < 0) { + //Serial.print("back "); + digitalWrite(pwm1, LOW); + digitalWrite(pwm2, HIGH); + } else { + //Serial.print("forward "); + digitalWrite(pwm1, HIGH); + digitalWrite(pwm2, LOW); + } + //Serial.println(sp); +} + +void loop() { + move(0); + //max + /* + Serial.println(); + Serial.println(); + Serial.println(); + move(maxspeed); + Serial.print("max speed: "); + delay(1000); + resetenc(); + delay(1000); + readenc(); + Serial.print(rpm); + Serial.print("rpm, with input: "); + Serial.println(maxspeed); + delay(250); + move(0); + + //min + delay(5000); + Serial.print("min speed: "); + for (int i = 0; i < maxspeed; i++) { + move(i); + delay(1000); + readenc(); + if ((rpm > 10) && (i>5)) { + Serial.print(rpm); + Serial.print("rpm, with input: "); + Serial.println(i); + break; + } + } + */ + delay(2000); + for (int i = 0; i < maxspeed; i++) { + Serial.println("0,0"); + } + int currentloopdata[maxspeed] = {}; + for (int i = 0; i < maxspeed; i++) { + move(i); + delay(100); + readenc(); + Serial.print(i); + Serial.print(","); + Serial.print(rpm); + currentloopdata[i] = rpm; + Serial.print(","); + Serial.println(totloopdata[i]); + } + /* + for (int i = 0; i < maxspeed; i++) { + move(0); + delay(200); + move(i); + delay(1000); + resetenc(); + delay(100); + readenc(); + Serial.print(i); + Serial.print(","); + Serial.print(rpm); + currentloopdata[i] = rpm; + Serial.print(","); + Serial.println(totloopdata[i]); + } + */ + currentloopdata[0] = 0; + if (Nloop == 0){ + for (int i = 0; i < maxspeed; i++) { + totloopdata[i] = currentloopdata[i]; + } + }else{ + for (int i = 0; i < maxspeed; i++) { + //Serial.print(currentloopdata[i]); + //Serial.print(", "); + //Serial.print(totloopdata[i]); + totloopdata[i] = ((totloopdata[i]*Nloop)+currentloopdata[i])/(Nloop+1); + } + } + for (int i = 0; i < maxspeed; i++) { + //Serial.print(currentloopdata[i]); + //Serial.print(", "); + } + + resetenc(); + SD.remove("avrage_motor_values.dat"); + File dataFile = SD.open("avrage_motor_values.dat", FILE_WRITE); + if (dataFile) { + for (int i = 0; i < maxspeed; i++) { + dataFile.print(totloopdata[i]); + dataFile.print(", "); + } + dataFile.close(); + } + SD.remove("avrage_motor_values.csv"); + File debugFile = SD.open("avrage_motor_values.csv", FILE_WRITE); + if (debugFile) { + for (int i = 0; i < maxspeed; i++) { + debugFile.print(i); + debugFile.print(", "); + debugFile.println(totloopdata[i]); + } + } + + if (!card.init(SPI_HALF_SPEED, chipSelect)) { + Serial.println("FAILED TO FIND SD CARD"); + Serial.println("* is a card inserted?"); + Serial.println("* is your wiring correct?"); + Serial.println("* did you change the chipSelect pin to match your shield or module?"); + } + + + Nloop++; + + +} diff --git a/CODE/debug/motordriver/motordriver.ino b/CODE/debug/motordriver/motordriver.ino new file mode 100644 index 0000000..2f1c225 --- /dev/null +++ b/CODE/debug/motordriver/motordriver.ino @@ -0,0 +1,386 @@ +#include <Adafruit_MCP23X17.h> +#include <TimerThree.h> +//moveohmi = https://stackoverflow.com/questions/3748037/how-to-control-a-kiwi-drive-robot +//https://www.pololu.com/product/2997 +//https://www.pololu.com/product/4861 +//https://ww1.microchip.com/downloads/en/devicedoc/20001952c.pdf + + +#define ArrayElementCount(needRPM) (sizeof(needRPM) / sizeof(needRPM[0])) + +// motor one/A front +#define m1enA 3 // GPA3 PWM1 +#define m1enB 2 // GPA2 +#define m1pwm1 0 // GPA0 +#define m1pwm2 1 // GPA1 +#define m1encA 12 // GPB4 +#define m1encB 13 // GPB5 +// motor two/B left +#define m2enA 7 // GPA7 PWM2 +#define m2enB 6 // GPA6 +#define m2pwm1 4 // GPA4 +#define m2pwm2 5 // GPA5 +#define m2encA 14 // GPB6 +#define m2encB 15 // GPB7 +// motor three/C right +#define m3enA 11 // GPB3 PWM3 +#define m3enB 10 // GPB2 +#define m3pwm1 8 // GPB0 +#define m3pwm2 9 // GPB1 +#define m3encA 24 // teensy port 24 +#define m3encB 9 // teensy port 9 + +double sp = 1200.000; //speed in RPM ~0-1800 +const int maxspeed = 1000; + +//calibration data +int Amotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910}; +int Bmotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910}; +int Cmotordata[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 2, 1, 0, 1, 1, 1, 0, 14, 12, 14, 9, 11, 16, 9, 9, 9, 16, 28, 28, 32, 42, 39, 29, 29, 45, 36, 30, 60, 102, 109, 113, 124, 154, 153, 154, 151, 172, 183, 214, 243, 288, 302, 306, 320, 336, 337, 338, 356, 420, 438, 451, 465, 501, 499, 492, 509, 519, 528, 561, 567, 611, 632, 610, 616, 638, 641, 641, 667, 705, 721, 735, 724, 772, 756, 761, 748, 773, 774, 810, 817, 849, 840, 840, 861, 858, 876, 857, 883, 917, 917, 920, 932, 954, 936, 925, 918, 963, 951, 960, 995, 1011, 1011, 1008, 1011, 1052, 1022, 1033, 1014, 1059, 1059, 1092, 1053, 1092, 1080, 1056, 1062, 1097, 1111, 1096, 1094, 1170, 1131, 1137, 1134, 1158, 1174, 1137, 1185, 1234, 1198, 1203, 1192, 1223, 1194, 1207, 1204, 1241, 1222, 1252, 1229, 1291, 1232, 1241, 1226, 1246, 1244, 1225, 1230, 1298, 1295, 1271, 1270, 1306, 1300, 1280, 1265, 1357, 1283, 1278, 1307, 1369, 1297, 1337, 1299, 1357, 1326, 1326, 1316, 1375, 1341, 1373, 1366, 1351, 1366, 1326, 1314, 1404, 1364, 1373, 1366, 1417, 1431, 1381, 1349, 1468, 1383, 1367, 1352, 1460, 1411, 1420, 1400, 1471, 1435, 1415, 1429, 1455, 1424, 1432, 1406, 1506, 1447, 1444, 1416, 1511, 1438, 1442, 1476, 1578, 1498, 1444, 1440, 1521, 1539, 1448, 1448, 1539, 1492, 1518, 1464, 1604, 1565, 1503, 1468, 1539, 1479, 1492, 1481, 1582, 1549, 1495, 1525, 1618, 1543, 1515, 1546, 1631, 1597, 1532, 1569, 1712, 1577, 1537, 1557, 1621, 1596, 1532, 1546, 1721, 1572, 1566, 1562, 1645, 1602, 1592, 1580, 1708, 1587, 1587, 1599, 1639, 1566, 1576, 1540, 1699, 1664, 1561, 1593, 1731, 1589, 1613, 1563, 1695, 1608, 1628, 1597, 1721, 1737, 1613, 1613, 1772, 1665, 1670, 1656, 1758, 1645, 1649, 1603, 1772, 1702, 1688, 1678, 1782, 1685, 1718, 1650, 1863, 1793, 1687, 1717, 1804, 1782, 1695, 1704, 1789, 1769, 1674, 1715, 1867, 1795, 1675, 1702, 1761, 1673, 1676, 1667, 1765, 1708, 1721, 1667, 1792, 1728, 1681, 1674, 1792, 1770, 1745, 1698, 1816, 1797, 1715, 1776, 1938, 1796, 1742, 1745, 1911, 1832, 1800, 1772, 1930, 1752, 1721, 1714, 1920, 1805, 1730, 1711, 1864, 1862, 1700, 1762, 1897, 1805, 1753, 1761, 1951, 1790, 1749, 1773, 1947, 1781, 1712, 1782, 1901, 1799, 1732, 1724, 1856, 1772, 1759, 1751, 1923, 1846, 1765, 1765, 1944, 1822, 1791, 1776, 1945, 1854, 1830, 1800, 1921, 1860, 1776, 1748, 1954, 1874, 1858, 1773, 2035, 1852, 1775, 1826, 2072, 1934, 1859, 1897, 2003, 2054, 1860, 1887, 2214, 1993, 1970, 1874, 2041, 2043, 1894, 1839, 2080, 1905, 1822, 1907, 2124, 1935, 1857, 1883, 1957, 1940, 1860, 1870, 1865, 1877, 1876, 1846, 2092, 1928, 1825, 1833, 1979, 1914, 1920, 1837, 1988, 1900, 1867, 1825, 2049, 1964, 1834, 1857, 1979, 1893, 1815, 1874, 2002, 2023, 1803, 1877, 1894, 1989, 1840, 1846, 2001, 1958, 1861, 1867, 2066, 1952, 1853, 1806, 1894, 1880, 1769, 1767, 1893, 1840, 1780, 1787, 1857, 1814, 1780, 1722, 1859, 1920, 1800, 1840, 1853, 1823, 1776, 1771, 1843, 1871, 1819, 1769, 1900, 1876, 1792, 1809, 1908, 1944, 1859, 1920, 2073, 1985, 1913, 1917, 1940, 1897, 1883, 1807, 1910, 1910, 1873, 1826, 1917, 1893, 1802, 1802, 1886, 2008, 1859, 1814, 1930, 1905, 1850, 1833, 1909, 2019, 1850, 1807, 2012, 2010, 1938, 1856, 2051, 1890, 1848, 1842, 1958, 1950, 1874, 1915, 2029, 1947, 1827, 1867, 1949, 2022, 1957, 1941, 2161, 2040, 2018, 1894, 2005, 2118, 1986, 1951, 1936, 1927, 1946, 1978, 2025, 2019, 1888, 1904, 1964, 1914, 1822, 1834, 1945, 1924, 1873, 1853, 1995, 1975, 1830, 1894, 1958, 1958, 1848, 1786, 1907, 1959, 1952, 1964, 2020, 1927, 1901, 1903, 1986, 1931, 1921, 1985, 1972, 1993, 2028, 1859, 1927, 1901, 1904, 1861, 1936, 1937, 1928, 1902, 1937, 1971, 1944, 1840, 1948, 1972, 1955, 1942, 1972, 2073, 2057, 1924, 1925, 2036, 1893, 1836, 2008, 2060, 1947, 1957, 2156, 1976, 1905, 1914, 2022, 1985, 1947, 1920, 2058, 1968, 1893, 1929, 2042, 2052, 1975, 1867, 2063, 1962, 2011, 1948, 2070, 1957, 1911, 1843, 1944, 1945, 1902, 1878, 1964, 1864, 1930, 1870, 1942, 1958, 1941, 1898, 1898, 1923, 1948, 1921, 1957, 1934, 1952, 1924, 1942, 1901, 1950, 1894, 2016, 2062, 1975, 1893, 1921, 1978, 1935, 1857, 1955, 1921, 1866, 1841, 1890, 1876, 1821, 1855, 1961, 1895, 1877, 1852, 1873, 1857, 1915, 1820, 1850, 1897, 1856, 1833, 1921, 1915, 1823, 1807, 1904, 1830, 1817, 1790, 1911, 1864, 1883, 1843, 1915, 1989, 1917, 1863, 1921, 1951, 1958, 1971, 1958, 1983, 1901, 1934, 1934, 1987, 2011, 1979, 2023, 2059, 2026, 1890, 2052, 1931, 1917, 1923, 2126, 2036, 1965, 1899, 1971, 1913, 1870, 1835, 1911, 1962, 2005, 1971, 2039, 2050, 1986, 1857, 1993, 1958, 2041, 2025, 1969, 2004, 2012, 1931, 2101, 1939, 2006, 2063, 2035, 2146, 1971, 1972, 2018, 2028, 1917, 1835, 1923, 1988, 1884, 1924, 1947, 1894, 1968, 1952, 1931, 1909, 1914, 1883, 1901, 1933, 2001, 1952, 1992, 2019, 1938, 1952, 1921, 2009, 1999, 1982, 1985, 1949, 1907, 1968, 1911, 2006, 1935, 1944, 1921, 1924, 1981, 1930, 1928, 2046, 1877, 1811, 1888, 1912, 1866, 1887, 1918, 1929, 1913, 1922, 1923, 1915, 1918, 1918, 1937, 1981, 1948, 1959, 1978, 1972, 1981, 1920, 1994, 1995, 1987, 1890, 1929, 1985, 1978, 2042, 2070, 2006, 1999, 1948, 1961, 2046, 1971, 1984, 2097, 1942, 1937, 1887, 2011, 1999, 2016, 1979, 2008, 1992, 2034, 2003, 1988, 2052, 2025, 1931, 2030, 1952, 1900, 1878, 1930, 2038, 1954, 1941, 2003, 2013, 1992, 1920, 2005, 1957, 1960, 1911, 1931, 1954, 1927, 1935, 1971, 2023, 1931, 1900, 1962, 1952, 1979, 1941, 1957, 1985, 1944, 1961, 2029, 1998, 1995, 1946, 1955, 1949, 1938, 1921, 1908, 1941, 1943, 1932, 1948, 1924, 1989, 1974, 1977, 1994, 2033, 1948, 1907, 1948, 1880, 1915, 1941, 1921, 1949, 1938, 1958, 1956, 1937, 1931, 2032, 1996, 2008, 1937, 1937, 1935, 1962, 1941, 1938, 2029, 2013, 1999, 2021, 2029, 1899, 1886, 2013, 1871, 1853, 1901, 1860, 1907, 1880, 1910, 1943, 1881, 1893, 1873, 1870, 1908, 1917, 1908, 1904, 1895, 1854, 1889, 1900, 1857, 1934, 1873, 1936, 1920, 1900, 1903, 1925, 1910, 1886, 1924, 1891, 1905, 1883, 1854, 1918, 1860, 1912, 1879, 1913, 1898, 1905, 1874, 1893, 1870, 1910}; + +int AneedRPM = 0; +int BneedRPM = 0; +int CneedRPM = 0; + + +//kiwidrive +double dirA = 0.00; +double dirB = 0.00; +double dirC = 0.00; +double spdA = 0.00; +double spdB = 0.00; +double spdC = 0.00; +double dirrad = 0.00; +double pi = 3.1415926535897; + +//encoder +double Arpm = 0.00; +volatile long Aencount = 0; +unsigned long ALtime = 0; +unsigned long ACtime = 0; +unsigned long APtime = 0; +double Brpm = 0.00; +volatile long Bencount = 0; +unsigned long BLtime = 0; +unsigned long BCtime = 0; +unsigned long BPtime = 0; +double Crpm = 0.00; +volatile long Cencount = 0; +unsigned long CLtime = 0; +unsigned long CCtime = 0; +unsigned long CPtime = 0; + +//softPWM +volatile int PWMval1 = 0; // PWM m1enA value 0-100 +volatile int PWMval2 = 0; // PWM m2enA value 0-100 +volatile int PWMval3 = 0; // PWM m3enA value 0-100 + + +Adafruit_MCP23X17 mcp2; + +void setup(){ + if (!mcp2.begin_I2C(0x22)) { + Serial.println("Error initializing MCP23017 at 0x22."); + //while (1); + } + // set all the motor control pins + mcp2.pinMode(m1enA, OUTPUT); + mcp2.pinMode(m1enB, OUTPUT); + mcp2.pinMode(m2enA, OUTPUT); + mcp2.pinMode(m2enB, OUTPUT); + mcp2.pinMode(m3enA, OUTPUT); + mcp2.pinMode(m3enB, OUTPUT); + mcp2.pinMode(m1pwm1, OUTPUT); + mcp2.pinMode(m1pwm2, OUTPUT); + mcp2.pinMode(m2pwm1, OUTPUT); + mcp2.pinMode(m2pwm2, OUTPUT); + mcp2.pinMode(m3pwm1, OUTPUT); + mcp2.pinMode(m3pwm2, OUTPUT); + + //set encoder pins + mcp2.pinMode(m1encA, INPUT); + mcp2.pinMode(m1encB, INPUT); + mcp2.pinMode(m2encA, INPUT); + mcp2.pinMode(m2encB, INPUT); + pinMode(m3encA, INPUT); + pinMode(m3encB, INPUT); + attachInterrupt(digitalPinToInterrupt(m1encA), AencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m1encB), AencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m2encA), BencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m2encB), BencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m3encA), CencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m3encB), CencoderISR, CHANGE); + + Timer3.initialize(800); // 800 microseconds or 1.25kHz + Timer3.attachInterrupt(updatePWM1); + Timer3.attachInterrupt(updatePWM2); + Timer3.attachInterrupt(updatePWM3); + +} +void updatePWM1() { + static int counter1 = 0; + + if (counter1 < PWMval1) { + mcp2.digitalWrite(m1enA, HIGH); + } else { + mcp2.digitalWrite(m1enA, LOW); + } + + counter1++; + if (counter1 >= 100) { + counter1 = 0; + } +} +void updatePWM2() { + static int counter2 = 0; + + if (counter2 < PWMval2) { + mcp2.digitalWrite(m2enA, HIGH); + } else { + mcp2.digitalWrite(m2enA, LOW); + } + + counter2++; + if (counter2 >= 100) { + counter2 = 0; + } +} +void updatePWM3() { + static int counter3 = 0; + + if (counter3 < PWMval3) { + mcp2.digitalWrite(m3enA, HIGH); + } else { + mcp2.digitalWrite(m3enA, LOW); + } + + counter3++; + if (counter3 >= 100) { + counter3 = 0; + } +} +void mcpsoftpwm(int pin, int vall){ + //M1enA = 3 = GPA3 + //M2enA = 7 = GPA7 + //M3enA = 11 = GPB3 + Serial.print("softPWM pin: "); + Serial.print(pin); + Serial.print(", value: "); + Serial.println(vall); + if (pin = 3){ + PWMval1 = vall; + Serial.print("Motor 1 enA PWM set"); + } + if (pin = 7){ + PWMval2 = vall; + Serial.print("Motor 2 enA PWM set"); + } + if (pin = 11){ + PWMval3 = vall; + Serial.print("Motor 3 enA PWM set"); + } + +} +void AencoderISR() { + Aencount++; +} +void BencoderISR() { + Bencount++; +} +void CencoderISR() { + Cencount++; +} +void Areadenc() { + ACtime = micros(); + APtime = ACtime - ALtime; + ALtime = ACtime; + Arpm = (Aencount / 211.2) * (60000000.0 / APtime); + Serial.print("Motor A RPM: "); + Serial.print(Arpm); + Serial.print(" Sample period:"); + Serial.print(APtime); + Serial.print("ms, encoder count:"); + Serial.println(Aencount); + Aencount = 0; +} +void Breadenc() { + BCtime = micros(); + BPtime = BCtime - BLtime; + BLtime = BCtime; + Brpm = (Bencount / 211.2) * (60000000.0 / BPtime); + Serial.print("Motor B RPM: "); + Serial.print(Brpm); + Serial.print(" Sample period: "); + Serial.print(BPtime); + Serial.print(" ms, encoder count: "); + Serial.println(Bencount); + Bencount = 0; +} +void Creadenc() { + CCtime = micros(); + CPtime = CCtime - CLtime; + CLtime = CCtime; + Crpm = (Cencount / 211.2) * (60000000.0 / CPtime); + Serial.print("Motor C RPM: "); + Serial.print(Crpm); + Serial.print(" Sample period: "); + Serial.print(CPtime); + Serial.print(" ms, encoder count: "); + Serial.println(Cencount); + Cencount = 0; +} +void Aresetenc(){ + ACtime = micros(); + ALtime = ACtime; + Aencount = 0; +} +void Bresetenc(){ + BCtime = micros(); + BLtime = BCtime; + Bencount = 0; +} +void Cresetenc(){ + CCtime = micros(); + CLtime = CCtime; + Cencount = 0; +} +void Amove(double Rsped) { + double sped = nearest(Rsped, Amotordata, ArrayElementCount(Amotordata)); + spdA = abs(sped) * 0.1; + mcpsoftpwm(m1enA, spdA); + mcp2.digitalWrite(m1enB, LOW); + if (sped < 0) { + Serial.print("Motor A back "); + mcp2.digitalWrite(m1pwm1, LOW); + mcp2.digitalWrite(m1pwm2, HIGH); + } else { + Serial.print("Motor A forward "); + mcp2.digitalWrite(m1pwm1, HIGH); + mcp2.digitalWrite(m1pwm2, LOW); + } + Serial.println(spdA); +} +void Bmove(double Rsped) { + double sped = nearest(Rsped, Bmotordata, ArrayElementCount(Bmotordata)); + spdB = abs(sped) * 0.1; + mcpsoftpwm(m2enA, spdB); + mcp2.digitalWrite(m2enB, LOW); + if (sped < 0) { + Serial.print("Motor B back "); + mcp2.digitalWrite(m2pwm1, LOW); + mcp2.digitalWrite(m2pwm2, HIGH); + } else { + Serial.print("Motor B forward "); + mcp2.digitalWrite(m2pwm1, HIGH); + mcp2.digitalWrite(m2pwm2, LOW); + } + Serial.println(spdB); +} +void Cmove(double Rsped) { + double sped = nearest(Rsped, Cmotordata, ArrayElementCount(Cmotordata)); + spdC = abs(sped) * 0.1; + mcpsoftpwm(m3enA, spdC); + mcp2.digitalWrite(m3enB, LOW); + if (sped < 0) { + Serial.print("Motor C back "); + mcp2.digitalWrite(m3pwm1, LOW); + mcp2.digitalWrite(m3pwm2, HIGH); + } else { + Serial.print("Motor C forward "); + mcp2.digitalWrite(m3pwm1, HIGH); + mcp2.digitalWrite(m3pwm2, LOW); + } + Serial.println(spdC); +} + +void moveohmi(int speed, int direction){ + Amove(0); + Bmove(0); + Cmove(0); + + double dirrad = (direction*71.0000) / 4068.0000; + Serial.println(dirrad); + //vector ontb in x en y + double y = sin(dirrad); + double x = cos(dirrad); + Serial.println((String)"x:"+x+" y:"+y); + double spdeA = y*speed; + double spdeB = (-0.500*x - 0.8660054*y)*speed; + double spdeC = (-0.500*x + 0.8660054*y)*speed; + Amove(spdeA); + Bmove(spdeB); + Cmove(spdeC); + Serial.println(); + Serial.println(); +} +void rotate(int rodeg, int speed){ + //use compas heading + //inacurate compas data +} +void demo(){ + Amove(sp); + Bmove(sp); + Cmove(sp); + Serial.println("seperate A B C rotation"); + delay(1000); + + Amove(0); + Bmove(0); + Cmove(0); + Serial.println("seperate A B C stop"); + delay(1000); + + Amove(-sp); + Bmove(-sp); + Cmove(-sp); + Serial.println("seperate A B C -rotation"); + delay(1000); + + moveohmi(0, 0); + Serial.println("ohmi stop"); + delay(1000); + + rotate(90, sp); + Serial.println("rotation function 90deg"); + rotate(90, sp); + Serial.println("rotation function -90deg"); + delay(1000); + + moveohmi(sp, 35); + Serial.println("ohmi move 35deg"); + delay(1000); + + moveohmi(sp, 240); + Serial.println("ohmi move 215deg"); + delay(1000); + + for(int i = 0; i<360; i++){ + moveohmi(sp, i); + Serial.println(i); + delay(50); + } + Serial.println("ohmi move circle"); + delay(1000); +} +void loop(){ + demo(); + delay(100); + +} +int nearest(int x, int myArray[], int elements) +{ + //https://forum.arduino.cc/t/search-for-nearest-number-in-an-array-help/245843/5 + int idx = 0; // by default near first element + + int distance = abs(myArray[idx] - x); + for (int i = 1; i < elements; i++) + { + int d = abs(myArray[i] - x); + if (d < distance) + { + idx = i; + distance = d; + } + } + return idx; +} diff --git a/CODE/debug/motordriver/nomultiplexer/nomultiplexer.ino b/CODE/debug/motordriver/nomultiplexer/nomultiplexer.ino new file mode 100644 index 0000000..f6f839c --- /dev/null +++ b/CODE/debug/motordriver/nomultiplexer/nomultiplexer.ino @@ -0,0 +1,254 @@ +//moveohmi = https://robotics.stackexchange.com/questions/7829/how-to-program-a-three-wheel-omni +//https://www.pololu.com/product/2997 +//https://www.pololu.com/product/4861 +// +// motor one/A front +int m1enA = 1; +int m1enB = 2; +int m1pwm1 = 3; +int m1pwm2 = 4; +int m1encA = 13; +int m1encB = 14; +// motor two/B left +int m2enA = 5; +int m2enB = 6; +int m2pwm1 = 7; +int m2pwm2 = 8; +int m2encA = 15; +int m2encB = 16; +// motor three/C right +int m3enA = 9; +int m3enB = 10; +int m3pwm1 = 11; +int m3pwm2 = 12; +int m3encA = 17; +int m3encB = 18; + +double sp = 1.000; //speed in 0-1 + +double dirA = 0.00; +double dirB = 0.00; +double dirC = 0.00; +double spdA = 0.00; +double spdB = 0.00; +double spdC = 0.00; +double dirrad = 0.00; +double pi = 3.1415926535897; + +double Arpm = 0.00; +volatile long Aencount = 0; +unsigned long ALtime = 0; +unsigned long ACtime = 0; +unsigned long APtime = 0; +double Brpm = 0.00; +volatile long Bencount = 0; +unsigned long BLtime = 0; +unsigned long BCtime = 0; +unsigned long BPtime = 0; +double Crpm = 0.00; +volatile long Cencount = 0; +unsigned long CLtime = 0; +unsigned long CCtime = 0; +unsigned long CPtime = 0; + +void setup() +{ + // set all the motor control pins to outputs + pinMode(m1enA, OUTPUT); + pinMode(m1enB, OUTPUT); + pinMode(m2enA, OUTPUT); + pinMode(m2enB, OUTPUT); + pinMode(m3enA, OUTPUT); + pinMode(m3enB, OUTPUT); + + pinMode(m1pwm1, OUTPUT); + pinMode(m1pwm2, OUTPUT); + pinMode(m2pwm1, OUTPUT); + pinMode(m2pwm2, OUTPUT); + pinMode(m3pwm1, OUTPUT); + pinMode(m3pwm2, OUTPUT); + + //set encoder pins to input + pinMode(m1encA, INPUT); + pinMode(m1encB, INPUT); + pinMode(m2encA, INPUT); + pinMode(m2encB, INPUT); + pinMode(m3encA, INPUT); + pinMode(m3encB, INPUT); + attachInterrupt(digitalPinToInterrupt(m1encA), AencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m1encB), AencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m2encA), BencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m2encB), BencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m3encA), CencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m3encB), CencoderISR, CHANGE); +} + +void AencoderISR() { + Aencount++; +} +void BencoderISR() { + Bencount++; +} +void CencoderISR() { + Cencount++; +} +void Areadenc() { + ACtime = micros(); + APtime = ACtime - ALtime; + ALtime = ACtime; + Arpm = (Aencount / 211.2) * (60000000.0 / APtime); + Serial.print("Motor A RPM: "); + Serial.print(Arpm); + Serial.print(" Sample period:"); + Serial.print(APtime); + Serial.print("ms, encoder count:"); + Serial.println(Aencount); + Aencount = 0; +} +void Breadenc() { + BCtime = micros(); + BPtime = BCtime - BLtime; + BLtime = BCtime; + Brpm = (Bencount / 211.2) * (60000000.0 / BPtime); + Serial.print("Motor B RPM: "); + Serial.print(Brpm); + Serial.print(" Sample period: "); + Serial.print(BPtime); + Serial.print(" ms, encoder count: "); + Serial.println(Bencount); + Bencount = 0; +} +void Creadenc() { + CCtime = micros(); + CPtime = CCtime - CLtime; + CLtime = CCtime; + Crpm = (Cencount / 211.2) * (60000000.0 / CPtime); + Serial.print("Motor C RPM: "); + Serial.print(Crpm); + Serial.print(" Sample period: "); + Serial.print(CPtime); + Serial.print(" ms, encoder count: "); + Serial.println(Cencount); + Cencount = 0; +} + +void Amove(double sped) { + spdA = abs(sped) * 255.000; + analogWrite(m1enA, spdA); + digitalWrite(m1enB, LOW); + if (sped < 0) { + Serial.print("Motor A back "); + digitalWrite(m1pwm1, LOW); + digitalWrite(m1pwm2, HIGH); + } else { + Serial.print("Motor A forward "); + digitalWrite(m1pwm1, HIGH); + digitalWrite(m1pwm2, LOW); + } + Serial.println(spdA); +} +void Bmove(double sped) { + spdB = abs(sped) * 255.000; + analogWrite(m2enA, spdB); + digitalWrite(m2enB, LOW); + if (sped < 0) { + Serial.print("Motor B back "); + digitalWrite(m2pwm1, LOW); + digitalWrite(m2pwm2, HIGH); + } else { + Serial.print("Motor B forward "); + digitalWrite(m2pwm1, HIGH); + digitalWrite(m2pwm2, LOW); + } + Serial.println(spdB); +} +void Cmove(double sped) { + spdC = abs(sped) * 255.000; + analogWrite(m3enA, spdC); + digitalWrite(m3enB, LOW); + if (sped < 0) { + Serial.print("Motor C back "); + digitalWrite(m3pwm1, LOW); + digitalWrite(m3pwm2, HIGH); + } else { + Serial.print("Motor C forward "); + digitalWrite(m3pwm1, HIGH); + digitalWrite(m3pwm2, LOW); + } + Serial.println(spdC); +} + + +void moveohmi(int speed, int direction){ + Amove(0); + Bmove(0); + Cmove(0); + + double dirrad = (direction*71.0000) / 4068.0000; + Serial.println(dirrad); + //vector ontb in x en y + double y = sin(dirrad); + double x = cos(dirrad); + Serial.println((String)"x:"+x+" y:"+y); + double spdeA = y*speed; + double spdeB = (-0.500*x - 0.8660054*y)*speed; + double spdeC = (-0.500*x + 0.8660054*y)*speed; + Amove(spdeA); + Bmove(spdeB); + Cmove(spdeC); + Serial.println(); + Serial.println(); +} +void rotate(int rodeg, int speed){ +//use compas heading +} +void demo(){ + Amove(sp); + Bmove(sp); + Cmove(sp); + Serial.println("seperate A B C rotation"); + delay(1000); + + Amove(0); + Bmove(0); + Cmove(0); + Serial.println("seperate A B C stop"); + delay(1000); + + Amove(-sp); + Bmove(-sp); + Cmove(-sp); + Serial.println("seperate A B C -rotation"); + delay(1000); + + moveohmi(0, 0); + Serial.println("ohmi stop"); + delay(1000); + + rotate(90, sp); + Serial.println("rotation function 90deg"); + rotate(90, sp); + Serial.println("rotation function -90deg"); + delay(1000); + + moveohmi(sp, 35); + Serial.println("ohmi move 35deg"); + delay(1000); + + moveohmi(sp, 240); + Serial.println("ohmi move 215deg"); + delay(1000); + + for(int i = 0; i<360; i++){ + moveohmi(sp, i); + Serial.println(i); + delay(50); + } + Serial.println("ohmi move circle"); + delay(1000); +} +void loop(){ + demo(); + delay(100); + +} diff --git a/CODE/debug/motordriver/onemotor/onemotor.ino b/CODE/debug/motordriver/onemotor/onemotor.ino new file mode 100644 index 0000000..33f0c5c --- /dev/null +++ b/CODE/debug/motordriver/onemotor/onemotor.ino @@ -0,0 +1,86 @@ +int enA = 1; +int enB = 2; +int pwm1 = 3; +int pwm2 = 4; +int encoA = 5; +int encoB = 6; + +int speed = 1; // speed 0-1 + +double sp = 0.00; +double rpm = 0.00; +volatile long encount = 0; +unsigned long Ltime = 0; +unsigned long Ctime = 0; +unsigned long Ptime = 0; + +void setup() { + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + + pinMode(encoA, INPUT_PULLUP); + pinMode(encoB, INPUT_PULLUP); + + attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE); + + Serial.begin(9600); +} + +void encoderISR() { + encount++; +} + +void readenc() { + Ctime = micros(); + Ptime = Ctime - Ltime; + Ltime = Ctime; + rpm = (encount / 211.2) * (60000000.0 / Ptime); + Serial.print("RPM: "); + Serial.print(rpm); + Serial.print(" Sample period:"); + Serial.print(Ptime); + Serial.print("ms, encoder count:"); + Serial.println(encount); + + encount = 0; +} +void resetenc(){ + Ctime = micros(); + Ltime = Ctime; + encount = 0; +} + +void move(int sped) { + sp = abs(sped)*255; + analogWrite(enA, sp); + digitalWrite(enB, LOW); + if (sped < 0) { + Serial.print("back "); + digitalWrite(pwm1, LOW); + digitalWrite(pwm2, HIGH); + } else { + Serial.print("forward "); + digitalWrite(pwm1, HIGH); + digitalWrite(pwm2, LOW); + } + Serial.println(sp); +} + +void loop() { + move(speed); + delay(1000); + readenc(); + + + move(0); + resetenc(); + delay(1000); + readenc(); + + move(-speed); + delay(1000); + readenc(); +} diff --git a/CODE/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino b/CODE/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino new file mode 100644 index 0000000..2f57d59 --- /dev/null +++ b/CODE/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino @@ -0,0 +1,126 @@ +#include <TimerThree.h> + +int enA = 1; //PWM pin +int enB = 2; //GND +int pwm1 = 3; // +int pwm2 = 4; +int encoA = 5; +int encoB = 6; + +int speed = 1000; // speed 0-1 + +double sp = 0.00; +double rpm = 0.00; +volatile long encount = 0; +unsigned long Ltime = 0; +unsigned long Ctime = 0; +unsigned long Ptime = 0; + +volatile int PWMval = 0; // PWM value 0-100 + + +void setup() { + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + + pinMode(encoA, INPUT_PULLUP); + pinMode(encoB, INPUT_PULLUP); + + attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE); + + Timer3.initialize(800); // 800 microseconds or 1.25kHz + Timer3.attachInterrupt(updatePWM); + + Serial.begin(9600); +} + +void updatePWM() { + static int counter = 0; + + if (counter < PWMval) { + digitalWrite(enA, HIGH); + } else { + digitalWrite(enA, LOW); + } + + counter++; + if (counter >= 100) { + counter = 0; + } +} +void mcpsoftpwm(int value) { + if (value < 0) value = 0; + if (value > 100) value = 100; + + Serial.print("softPWM pin: enA, value: "); + Serial.println(value); + PWMval = value; +} + +void encoderISR() { + encount++; +} + +void readenc() { + Ctime = micros(); + Ptime = Ctime - Ltime; + Ltime = Ctime; + rpm = (encount / 211.2) * (60000000.0 / Ptime); + Serial.print("RPM: "); + Serial.print(rpm); + Serial.print(" Sample period:"); + Serial.print(Ptime); + Serial.print("ms, encoder count:"); + Serial.println(encount); + + encount = 0; +} +void resetenc(){ + Ctime = micros(); + Ltime = Ctime; + encount = 0; +} + +void move(int sped) { + sp = abs(sped)*0.1; + mcpsoftpwm(sp); + digitalWrite(enB, LOW); + if (sped < 0) { + Serial.print("back "); + digitalWrite(pwm1, LOW); + digitalWrite(pwm2, HIGH); + } else { + Serial.print("forward "); + digitalWrite(pwm1, HIGH); + digitalWrite(pwm2, LOW); + } + Serial.println(sp); +} + +void loop() { + move(speed); + delay(1000); + readenc(); + delay(1000); + + Serial.println(); + + move(0); + delay(1000); + resetenc(); + delay(1000); + readenc(); + delay(1000); + + Serial.println(); + + move(500); + delay(1000); + readenc(); + delay(1000); + + Serial.println(); +} diff --git a/CODE/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino b/CODE/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino new file mode 100644 index 0000000..5c8ff2a --- /dev/null +++ b/CODE/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino @@ -0,0 +1,128 @@ +#include <TimerThree.h> +#include <Adafruit_MCP23X17.h> + +//https://www.pololu.com/product/2997 +//https://www.pololu.com/product/4861 +//https://ww1.microchip.com/downloads/en/devicedoc/20001952c.pdf + +Adafruit_MCP23X17 mcp; + +int enA = 0; // GPA0 -PWM +int enB = 1; // GPA1 -GND +int pwm1 = 2; // GPA2 -HIGH/LOW +int pwm2 = 3; // GPA3 -HIGH/LOW +int encoA = 4; // GPA4 -read +int encoB = 5; // GPA5 -read + +int speed = 100; // speed 0-1 + +double sp = 0.00; +double rpm = 0.00; +volatile long encount = 0; +unsigned long Ltime = 0; +unsigned long Ctime = 0; +unsigned long Ptime = 0; + +volatile int PWMval = 0; // PWM value 0-100 + + +void setup() { + Serial.begin(9600); + if (!mcp.begin_I2C(0x20)) { + Serial.println("Error initializing MCP23017 at 0x20."); + while (1); + } + + mcp.pinMode(enA, OUTPUT); + mcp.pinMode(enB, OUTPUT); + mcp.pinMode(pwm1, OUTPUT); + mcp.pinMode(pwm2, OUTPUT); + + mcp.pinMode(encoA, INPUT_PULLUP); + mcp.pinMode(encoB, INPUT_PULLUP); + + attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE); + + Timer3.initialize(800); // 800 microseconds or 1.25kHz + Timer3.attachInterrupt(updatePWM); +} + +void updatePWM() { + static int counter = 0; + + if (counter < PWMval) { + mcp.digitalWrite(enA, HIGH); + } else { + mcp.digitalWrite(enA, LOW); + } + + counter++; + if (counter >= 100) { + counter = 0; + } +} +void mcpsoftpwm(int value) { + if (value < 0) value = 0; + if (value > 100) value = 100; + + Serial.print("softPWM pin: enA, value: "); + Serial.println(value); + PWMval = value; +} + +void encoderISR() { + encount++; +} + +void readenc() { + Ctime = micros(); + Ptime = Ctime - Ltime; + Ltime = Ctime; + rpm = (encount / 211.2) * (60000000.0 / Ptime); + Serial.print("RPM: "); + Serial.print(rpm); + Serial.print(" Sample period:"); + Serial.print(Ptime); + Serial.print("ms, encoder count:"); + Serial.println(encount); + + encount = 0; +} +void resetenc(){ + Ctime = micros(); + Ltime = Ctime; + encount = 0; +} + +void move(int sped) { + sp = abs(sped)*1; + mcpsoftpwm(sp); + mcp.digitalWrite(enB, LOW); + if (sped < 0) { + Serial.print("back "); + mcp.digitalWrite(pwm1, LOW); + mcp.digitalWrite(pwm2, HIGH); + } else { + Serial.print("forward "); + mcp.digitalWrite(pwm1, HIGH); + mcp.digitalWrite(pwm2, LOW); + } + Serial.println(sp); +} + +void loop() { + move(speed); + delay(1000); + readenc(); + + + move(0); + resetenc(); + delay(1000); + readenc(); + + move(-speed); + delay(1000); + readenc(); +} diff --git a/CODE/debug/motordriver/softPWM/softPWM.ino b/CODE/debug/motordriver/softPWM/softPWM.ino new file mode 100644 index 0000000..ab74715 --- /dev/null +++ b/CODE/debug/motordriver/softPWM/softPWM.ino @@ -0,0 +1,46 @@ +#include <TimerThree.h> +#include <Arduino.h> + +int pwmPin = 3; + +volatile int PWMval = 0; // PWM value 0-100 + +void setup() { + pinMode(pwmPin, OUTPUT); + Serial.begin(9600); + + Timer3.initialize(800); // 800 microseconds or 1.25kHz + Timer3.attachInterrupt(updatePWM); +} + +void updatePWM() { + static int counter = 0; + + if (counter < PWMval) { + digitalWrite(pwmPin, HIGH); + } else { + digitalWrite(pwmPin, LOW); + } + + counter++; + if (counter >= 100) { + counter = 0; + } +} + +void mcpsoftpwm(int value) { + if (value < 0) value = 0; + if (value > 100) value = 100; + + Serial.print("softPWM pin: 3, value: "); + Serial.println(value); + PWMval = value; +} + +void loop() { + mcpsoftpwm(100); // Set PWM to 100% + delay(1000); + mcpsoftpwm(50); // Set PWM to 50% + delay(1000); + Serial.println("loop"); +} diff --git a/CODE/debug/motordriver/tree_motor_calibration/tree_motor_calibration.ino b/CODE/debug/motordriver/tree_motor_calibration/tree_motor_calibration.ino new file mode 100644 index 0000000..4608903 --- /dev/null +++ b/CODE/debug/motordriver/tree_motor_calibration/tree_motor_calibration.ino @@ -0,0 +1,427 @@ +#include <TimerThree.h> +#include <Adafruit_MCP23X17.h> +#include <SD.h> +#include <SPI.h> + +//https://www.pololu.com/product/2997 +//https://www.pololu.com/product/4861 +//https://ww1.microchip.com/downloads/en/devicedoc/20001952c.pdf + +// motor one/A front +#define m1enA 3 // GPA3 PWM1 +#define m1enB 2 // GPA2 +#define m1pwm1 0 // GPA0 +#define m1pwm2 1 // GPA1 +#define m1encA 12 // GPB4 +#define m1encB 13 // GPB5 +// motor two/B left +#define m2enA 7 // GPA7 PWM2 +#define m2enB 6 // GPA6 +#define m2pwm1 4 // GPA4 +#define m2pwm2 5 // GPA5 +#define m2encA 14 // GPB6 +#define m2encB 15 // GPB7 +// motor three/C right +#define m3enA 11 // GPB3 PWM3 +#define m3enB 10 // GPB2 +#define m3pwm1 8 // GPB0 +#define m3pwm2 9 // GPB1 +#define m3encA 24 // teensy port 24 +#define m3encB 9 // teensy port 9 + +const int maxspeed = 1000; + +double sp = 0.00; +//encoder +double Arpm = 0.00; +volatile long Aencount = 0; +unsigned long ALtime = 0; +unsigned long ACtime = 0; +unsigned long APtime = 0; +double Brpm = 0.00; +volatile long Bencount = 0; +unsigned long BLtime = 0; +unsigned long BCtime = 0; +unsigned long BPtime = 0; +double Crpm = 0.00; +volatile long Cencount = 0; +unsigned long CLtime = 0; +unsigned long CCtime = 0; +unsigned long CPtime = 0; + +//kiwidrive +double dirA = 0.00; +double dirB = 0.00; +double dirC = 0.00; +double spdA = 0.00; +double spdB = 0.00; +double spdC = 0.00; +double dirrad = 0.00; +double pi = 3.1415926535897; + +//softPWM +volatile int PWMval1 = 0; // PWM m1enA value 0-100 +volatile int PWMval2 = 0; // PWM m2enA value 0-100 +volatile int PWMval3 = 0; // PWM m3enA value 0-100 + +volatile int PWMval = 0; // PWM value 0-100 +File ; +File avrage; +Sd2Card card; +SdVolume volume; +SdFile root; +const int chipSelect = BUILTIN_SDCARD; +int x = 1; + +int Nloop = 0; +int Atotloopdata[maxspeed] = {}; +int Btotloopdata[maxspeed] = {}; +int Ctotloopdata[maxspeed] = {}; + +Adafruit_MCP23X17 mcp2; + +void setup() { + if (!mcp2.begin_I2C(0x22)) { + Serial.println("Error initializing MCP23017 at 0x22."); + //while (1); + } + // set all the motor control pins + mcp2.pinMode(m1enA, OUTPUT); + mcp2.pinMode(m1enB, OUTPUT); + mcp2.pinMode(m2enA, OUTPUT); + mcp2.pinMode(m2enB, OUTPUT); + mcp2.pinMode(m3enA, OUTPUT); + mcp2.pinMode(m3enB, OUTPUT); + mcp2.pinMode(m1pwm1, OUTPUT); + mcp2.pinMode(m1pwm2, OUTPUT); + mcp2.pinMode(m2pwm1, OUTPUT); + mcp2.pinMode(m2pwm2, OUTPUT); + mcp2.pinMode(m3pwm1, OUTPUT); + mcp2.pinMode(m3pwm2, OUTPUT); + + //set encoder pins + mcp2.pinMode(m1encA, INPUT); + mcp2.pinMode(m1encB, INPUT); + mcp2.pinMode(m2encA, INPUT); + mcp2.pinMode(m2encB, INPUT); + pinMode(m3encA, INPUT); + pinMode(m3encB, INPUT); + attachInterrupt(digitalPinToInterrupt(m1encA), AencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m1encB), AencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m2encA), BencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m2encB), BencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m3encA), CencoderISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(m3encB), CencoderISR, CHANGE); + + Timer3.initialize(800); // 800 microseconds or 1.25kHz + Timer3.attachInterrupt(updatePWM1); + Timer3.attachInterrupt(updatePWM2); + Timer3.attachInterrupt(updatePWM3); +} + +void updatePWM1() { + static int counter1 = 0; + + if (counter1 < PWMval1) { + mcp2.digitalWrite(m1enA, HIGH); + } else { + mcp2.digitalWrite(m1enA, LOW); + } + + counter1++; + if (counter1 >= 100) { + counter1 = 0; + } +} +void updatePWM2() { + static int counter2 = 0; + + if (counter2 < PWMval2) { + mcp2.digitalWrite(m2enA, HIGH); + } else { + mcp2.digitalWrite(m2enA, LOW); + } + + counter2++; + if (counter2 >= 100) { + counter2 = 0; + } +} +void updatePWM3() { + static int counter3 = 0; + + if (counter3 < PWMval3) { + mcp2.digitalWrite(m3enA, HIGH); + } else { + mcp2.digitalWrite(m3enA, LOW); + } + + counter3++; + if (counter3 >= 100) { + counter3 = 0; + } +} +void mcpsoftpwm(int pin, int vall){ + //M1enA = 3 = GPA3 + //M2enA = 7 = GPA7 + //M3enA = 11 = GPB3 + Serial.print("softPWM pin: "); + Serial.print(pin); + Serial.print(", value: "); + Serial.println(vall); + if (pin = 3){ + PWMval1 = vall; + Serial.print("Motor 1 enA PWM set"); + } + if (pin = 7){ + PWMval2 = vall; + Serial.print("Motor 2 enA PWM set"); + } + if (pin = 11){ + PWMval3 = vall; + Serial.print("Motor 3 enA PWM set"); + } + +} + +void AencoderISR() { + Aencount++; +} +void BencoderISR() { + Bencount++; +} +void CencoderISR() { + Cencount++; +} + +void Areadenc() { + ACtime = micros(); + APtime = ACtime - ALtime; + ALtime = ACtime; + Arpm = (Aencount / 211.2) * (60000000.0 / APtime); + Serial.print("Motor A RPM: "); + Serial.print(Arpm); + Serial.print(" Sample period:"); + Serial.print(APtime); + Serial.print("ms, encoder count:"); + Serial.println(Aencount); + Aencount = 0; +} +void Breadenc() { + BCtime = micros(); + BPtime = BCtime - BLtime; + BLtime = BCtime; + Brpm = (Bencount / 211.2) * (60000000.0 / BPtime); + Serial.print("Motor B RPM: "); + Serial.print(Brpm); + Serial.print(" Sample period: "); + Serial.print(BPtime); + Serial.print(" ms, encoder count: "); + Serial.println(Bencount); + Bencount = 0; +} +void Creadenc() { + CCtime = micros(); + CPtime = CCtime - CLtime; + CLtime = CCtime; + Crpm = (Cencount / 211.2) * (60000000.0 / CPtime); + Serial.print("Motor C RPM: "); + Serial.print(Crpm); + Serial.print(" Sample period: "); + Serial.print(CPtime); + Serial.print(" ms, encoder count: "); + Serial.println(Cencount); + Cencount = 0; +} +void Aresetenc(){ + ACtime = micros(); + ALtime = ACtime; + Aencount = 0; +} +void Bresetenc(){ + BCtime = micros(); + BLtime = BCtime; + Bencount = 0; +} +void Cresetenc(){ + CCtime = micros(); + CLtime = CCtime; + Cencount = 0; +} + +void Amove(double sped) { + spdA = abs(sped) * 0.1; + mcpsoftpwm(m1enA, spdA); + mcp2.digitalWrite(m1enB, LOW); + if (sped < 0) { + //Serial.print("Motor A back "); + mcp2.digitalWrite(m1pwm1, LOW); + mcp2.digitalWrite(m1pwm2, HIGH); + } else { + //Serial.print("Motor A forward "); + mcp2.digitalWrite(m1pwm1, HIGH); + mcp2.digitalWrite(m1pwm2, LOW); + } + //Serial.println(spdA); +} +void Bmove(double sped) { + spdB = abs(sped) * 0.1; + mcpsoftpwm(m2enA, spdB); + mcp2.digitalWrite(m2enB, LOW); + if (sped < 0) { + //Serial.print("Motor B back "); + mcp2.digitalWrite(m2pwm1, LOW); + mcp2.digitalWrite(m2pwm2, HIGH); + } else { + //Serial.print("Motor B forward "); + mcp2.digitalWrite(m2pwm1, HIGH); + mcp2.digitalWrite(m2pwm2, LOW); + } + //Serial.println(spdB); +} +void Cmove(double sped) { + spdC = abs(sped) * 0.1; + mcpsoftpwm(m3enA, spdC); + mcp2.digitalWrite(m3enB, LOW); + if (sped < 0) { + //Serial.print("Motor C back "); + mcp2.digitalWrite(m3pwm1, LOW); + mcp2.digitalWrite(m3pwm2, HIGH); + } else { + //Serial.print("Motor C forward "); + mcp2.digitalWrite(m3pwm1, HIGH); + mcp2.digitalWrite(m3pwm2, LOW); + } + //Serial.println(spdC); +} +void move(double sped) { + Amove(sped); + Bmove(sped); + Cmove(sped); +} +void readenc() { + Areadenc(); + Breadenc(); + Creadenc(); +} +void resetenc() { + Aresetenc(); + Bresetenc(); + Cresetenc(); +} + +void loop() { + move(0); + delay(2000); + for (int i = 0; i < maxspeed; i++) { + Serial.println("0,0"); + } + int Acurrentloopdata[maxspeed] = {}; + int Bcurrentloopdata[maxspeed] = {}; + int Ccurrentloopdata[maxspeed] = {}; + for (int i = 0; i < maxspeed; i++) { + move(i); + delay(100); + readenc(); + Serial.print(i); + Serial.print(","); + Serial.print(Arpm); + Acurrentloopdata[i] = Arpm; + Serial.print(","); + Serial.print(Brpm); + Acurrentloopdata[i] = Brpm; + Serial.print(","); + Serial.print(Crpm); + Acurrentloopdata[i] = Crpm; + Serial.print(","); + Serial.println(Atotloopdata[i]); + Serial.print(","); + Serial.println(Btotloopdata[i]); + Serial.print(","); + Serial.println(Ctotloopdata[i]); + } + Acurrentloopdata[0] = 0; + Bcurrentloopdata[0] = 0; + Ccurrentloopdata[0] = 0; + if (Nloop == 0){ + for (int i = 0; i < maxspeed; i++) { + Atotloopdata[i] = Acurrentloopdata[i]; + Btotloopdata[i] = Bcurrentloopdata[i]; + Ctotloopdata[i] = Ccurrentloopdata[i]; + } + }else{ + for (int i = 0; i < maxspeed; i++) { + Atotloopdata[i] = ((Atotloopdata[i]*Nloop)+Acurrentloopdata[i])/(Nloop+1); + Btotloopdata[i] = ((Btotloopdata[i]*Nloop)+Bcurrentloopdata[i])/(Nloop+1); + Ctotloopdata[i] = ((Ctotloopdata[i]*Nloop)+Ccurrentloopdata[i])/(Nloop+1); + } + } + resetenc(); + SD.remove("Aavrage_motor_values.dat"); + SD.remove("Bavrage_motor_values.dat"); + SD.remove("Cavrage_motor_values.dat"); + File AdataFile = SD.open("Aavrage_motor_values.dat", FILE_WRITE); + if (AdataFile) { + for (int i = 0; i < maxspeed; i++) { + AdataFile.print(Atotloopdata[i]); + AdataFile.print(", "); + } + AdataFile.close(); + } + File BdataFile = SD.open("Aavrage_motor_values.dat", FILE_WRITE); + if (BdataFile) { + for (int i = 0; i < maxspeed; i++) { + BdataFile.print(Btotloopdata[i]); + BdataFile.print(", "); + } + BdataFile.close(); + } + File CdataFile = SD.open("Aavrage_motor_values.dat", FILE_WRITE); + if (CdataFile) { + for (int i = 0; i < maxspeed; i++) { + CdataFile.print(Ctotloopdata[i]); + CdataFile.print(", "); + } + CdataFile.close(); + } + SD.remove("ABC_avrage_motor_values.csv"); + File debugFile = SD.open("ABC_avrage_motor_values.csv", FILE_WRITE); + if (debugFile) { + debugFile.println(""); + debugFile.println("Motor A values"); + debugFile.println(""); + for (int i = 0; i < maxspeed; i++) { + debugFile.print(i); + debugFile.print(", "); + debugFile.println(Atotloopdata[i]); + } + debugFile.println(""); + debugFile.println("Motor B values"); + debugFile.println(""); + for (int i = 0; i < maxspeed; i++) { + debugFile.print(i); + debugFile.print(", "); + debugFile.println(Btotloopdata[i]); + } + debugFile.println(""); + debugFile.println("Motor C values"); + debugFile.println(""); + for (int i = 0; i < maxspeed; i++) { + debugFile.print(i); + debugFile.print(", "); + debugFile.println(Ctotloopdata[i]); + } + } + + if (!card.init(SPI_HALF_SPEED, chipSelect)) { + Serial.println("FAILED TO FIND SD CARD"); + Serial.println("* is a card inserted?"); + Serial.println("* is your wiring correct?"); + Serial.println("* did you change the chipSelect pin to match your shield or module?"); + } + + + Nloop++; + + +} |
