/****************************************************************************
* FasterDriver by (c) 2018 Marcio Teixeira *
* *
* This program is free software: you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* To view a copy of the GNU General Public License, go to the following *
* location: . *
****************************************************************************/
#include
#define LED_PIN 13
#define POT_PIN A6
#define X_APWM_PIN 10 // PB1 Timer 1
#define Y_BPWM_PIN 9 // PB2 Timer 1
#define X_AIN1_PIN 3
#define X_AIN2_PIN 2
#define Y_BIN1_PIN 4
#define Y_BIN2_PIN 8
// Use direct register access for the PWM
// pins for better frame rates.
#define USE_ARDUINO_NANO_FAST_IO
PROGMEM const char heart[] =
{ 0x00, 0xDA, 0x00, 0x25, 0xDD, 0x77, 0xF7, 0xDD, 0x7D, 0x77,
0xDF, 0xDD, 0x77, 0xF7, 0xDD, 0x7D, 0x77, 0xDF, 0xDD, 0x77,
0xF7, 0xDD, 0x7D, 0xF7, 0xDD, 0x7D, 0x77, 0xDF, 0xDD, 0x77,
0xF7, 0xDD, 0x7D, 0x77, 0xDF, 0xDD, 0x77, 0xF7, 0xDD, 0x7D,
0x77, 0xDF, 0x7D, 0x77, 0xDF, 0xDD, 0x77, 0xF7, 0xDD, 0x7D,
0x77, 0xDF, 0xDD, 0x77, 0xF7, 0xDD, 0x7D, 0x77, 0xDF, 0xDD,
0x77, 0xF7, 0xDD, 0x77, 0xF7, 0xDD, 0x7D, 0x77, 0xDF, 0xDD,
0x77, 0xF7, 0xDD, 0x7D, 0x77, 0xDF, 0xDD, 0x77, 0xF7, 0xDD,
0x7D, 0x77, 0xDF, 0x7D, 0x77, 0xDF, 0xDD, 0x77, 0xF7, 0xDD,
0x7D, 0x77, 0xDF, 0xDD, 0x77, 0xF7, 0xDD, 0x7D, 0x77, 0xDF,
0xDD, 0x77, 0xDF, 0xDD, 0x77, 0xF7, 0xDD, 0x7D, 0x77, 0xDF,
0xDD, 0x77, 0xF7, 0xDD, 0x7D, 0x77, 0xDF, 0xDD, 0x77, 0xF7,
0xDD, 0x7D, 0xF7, 0xDD, 0x7D, 0x77, 0xDF, 0xDD, 0x77, 0xF7,
0xDD, 0x6D, 0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99, 0x69, 0x66,
0x9A, 0x69, 0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99, 0x69, 0x66,
0x9A, 0x99, 0x66, 0xA6, 0x99, 0x69, 0x66, 0x9A, 0x99, 0x66,
0xA6, 0x99, 0x66, 0xA6, 0x99, 0x69, 0x66, 0x9A, 0x99, 0x66,
0xA6, 0x99, 0x69, 0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99, 0x69,
0x66, 0x9A, 0x69, 0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99, 0x69,
0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99, 0x69, 0x66, 0x9A, 0x99,
0x66, 0xA6, 0x99, 0x66, 0xA6, 0x99, 0x69, 0x66, 0x9A, 0x99,
0x66, 0xA6, 0x99, 0x69, 0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99,
0x69, 0xA6, 0x99, 0x69, 0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99,
0x69, 0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99, 0x69, 0x66, 0x9A,
0x99, 0x66, 0x9A, 0x99, 0x66, 0xA6, 0x99, 0x69, 0x66, 0x9A,
0x99, 0x66, 0xA6, 0x99, 0x69, 0x66, 0x9A, 0x99, 0x66, 0x66,
0x66, 0xA6, 0x99, 0x99, 0x69, 0x66, 0x9A, 0x69, 0xA6, 0x99,
0xA6, 0x69, 0xA6, 0x69, 0x6A, 0x9A, 0xA6, 0xA6, 0xA9, 0xA9,
0xA9, 0xA6, 0x9A, 0x6A, 0xAA, 0x9A, 0xAA, 0x6A, 0xAA, 0xAA,
0xAA, 0xAA, 0xAA, 0x2A, 0xAA, 0xAA, 0xA8, 0x2A, 0xAA, 0xA8,
0xA2, 0x8A, 0x8A, 0x8A, 0xA2, 0xA2, 0x28, 0x2A, 0x8A, 0x22,
0x8A, 0xA2, 0x88, 0x22, 0x8A, 0x28, 0x22, 0x8A, 0x88, 0x88,
0x22, 0x22, 0x22, 0x22, 0x22, 0x82, 0x88, 0x88, 0x20, 0x22,
0x88, 0x20, 0x82, 0x08, 0x82, 0x20, 0x82, 0x20, 0x20, 0x08,
0x02, 0x82, 0x80, 0x80, 0x00, 0x02, 0x08, 0x20, 0x00, 0x08,
0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x00, 0xC0,
0x00, 0x30, 0xC0, 0x00, 0x03, 0x0C, 0x0C, 0x0C, 0x03, 0xC3,
0x30, 0x30, 0x0C, 0x33, 0x0C, 0xC3, 0x0C, 0x33, 0xCC, 0x30,
0x33, 0xCC, 0xCC, 0x0C, 0x33, 0x33, 0x33, 0x33, 0x33, 0xCF,
0xCC, 0xCC, 0x33, 0xF3, 0xCC, 0x33, 0xCF, 0x3C, 0xCF, 0x33,
0xCF, 0xF3, 0xF3, 0x3C, 0x3F, 0xCF, 0xCF, 0xCF, 0x3F, 0xFF,
0xFC, 0xF3, 0xFF, 0xFC, 0xFF, 0xF3, 0xFF, 0xFF, 0xAB, 0xAA,
0xAA, 0xA8, 0xAA, 0xA2, 0xAA, 0xA8, 0xA2, 0x8A, 0x2A, 0x2A,
0x2A, 0x8A, 0x8A, 0xA2, 0xA8, 0x28, 0x8A, 0x28, 0x8A, 0x22,
0x8A, 0x28, 0xA2, 0x88, 0x28, 0x22, 0x22, 0x8A, 0x88, 0x88,
0x88, 0x88, 0x08, 0x22, 0x22, 0x82, 0x88, 0x20, 0x82, 0x08,
0x22, 0x08, 0x82, 0x08, 0x82, 0x80, 0x20, 0x08, 0x08, 0x02,
0x02, 0x02, 0x08, 0x20, 0x80, 0x00, 0x20, 0x00, 0x80, 0x00,
0x00, 0x00, 0x00, 0x00, 0xC0, 0x00, 0x00, 0x03, 0xC0, 0x00,
0x03, 0x0C, 0x30, 0x30, 0x30, 0x0C, 0x0C, 0xC3, 0xC0, 0x30,
0xCC, 0x30, 0x0C, 0x33, 0xCC, 0x30, 0xC3, 0xCC, 0x30, 0x33,
0x33, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0x3C, 0x33, 0x33, 0xCF,
0xCC, 0x33, 0xCF, 0x3C, 0xF3, 0x3C, 0xCF, 0x3C, 0xCF, 0xCF,
0xF3, 0xFC, 0x3C, 0x3F, 0x3F, 0xFF, 0xFC, 0xF3, 0xCF, 0xFF,
0xF3, 0xFF, 0xCF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xDF, 0xFF,
0x7F, 0xFF, 0xDF, 0x7F, 0xFF, 0xFD, 0xF7, 0xF7, 0xF7, 0xFD,
0x7D, 0xDF, 0xDF, 0xF7, 0xDD, 0xF7, 0x7D, 0xF7, 0xDD, 0x77,
0xDF, 0xDD, 0x77, 0x77, 0xF7, 0xDD, 0xDD, 0x0D };
/******************************* PWM FREQUENCY *******************************/
#define PWM_FREQ_31372Hz 0x01 // Sets the PWM frequency to 31372.55 Hz
#define PWM_FREQ_3921Hz 0x02 // Sets the PWM frequency to 3921.16 Hz
#define PWM_FREQ_980Hz 0x03 // Sets the PWM frequency to 980.39 Hz
// Allows adjustment of the PWM frequency. Higher values will allow
// for virtually silent operation at low speeds.
void setPWMTimerFrequencies(uint8_t frequency) {
// Reference: http://playground.arduino.cc/Main/TimerPWMCheatsheet
TCCR1B = (TCCR1B & 0b11111000) | frequency; // Set timer1 (pins 9 & 10) frequency
}
/******************************* DRIVER CONTROL *******************************/
void setDriverPins() {
pinMode(LED_PIN, OUTPUT);
pinMode(X_APWM_PIN, OUTPUT);
pinMode(Y_BPWM_PIN, OUTPUT);
pinMode(X_AIN1_PIN, OUTPUT);
pinMode(Y_BIN1_PIN, OUTPUT);
pinMode(X_AIN2_PIN, OUTPUT);
pinMode(Y_BIN2_PIN, OUTPUT);
analogWrite(X_APWM_PIN, 1);
analogWrite(Y_BPWM_PIN, 1);
}
#if defined(USE_ARDUINO_NANO_FAST_IO) && (X_APWM_PIN == 10) && (Y_BPWM_PIN == 9)
#pragma message "Using FastIO for PWM"
#define WRITE_PWM_X(x) OCR1B = x;
#define WRITE_PWM_Y(y) OCR1A = y;
#else
#pragma message "Using analogWrite for PWM"
#define WRITE_PWM_X(x) analogWrite(X_APWM_PIN, x);
#define WRITE_PWM_Y(y) analogWrite(Y_APWM_PIN, y);
#endif
// This function adjusts the pins for the TB6612FNG motor driver such that
// the specified amplitude shows up in the output
// This function adjusts the pins for the TB6612FNG motor driver such that
// the specified amplitude shows up in the output
void setAmplitude(int16_t x, int16_t y, uint8_t scale) {
static bool last_sign_x, last_sign_y;
static uint8_t last_pwm_x, last_pwm_y;
const bool sign_y = y > 0;
const bool sign_x = x > 0;
uint8_t x_pwm = abs(x), y_pwm = abs(y);
nscale8x2(x_pwm, y_pwm, scale);
if(last_pwm_x != x_pwm) {WRITE_PWM_X(x_pwm); last_pwm_x = x_pwm;}
if(last_pwm_y != y_pwm) {WRITE_PWM_Y(y_pwm); last_pwm_y = y_pwm;}
// Update the direction of the X driver
if(sign_x != last_sign_x) {
last_sign_x = sign_x;
digitalWrite(X_AIN1_PIN, HIGH);
digitalWrite(X_AIN2_PIN, HIGH);
digitalWrite(sign_x ? X_AIN1_PIN : X_AIN2_PIN, LOW);
}
// Update the direction of the Y driver
if(sign_y != last_sign_y) {
last_sign_y = sign_y;
digitalWrite(Y_BIN1_PIN, HIGH);
digitalWrite(Y_BIN2_PIN, HIGH);
digitalWrite(sign_y ? Y_BIN1_PIN : Y_BIN2_PIN, LOW);
}
}
/******************************* MAX AMPLITUDE *******************************/
// Read the amplitude from the potentiometer
uint8_t getMaxAmplitude() {
static uint8_t val;
static uint8_t poll = 0;
if(poll++ == 0) {
val = analogRead(POT_PIN) / 1023.0f * 255;
if(val) {
#if defined(STNDBY_PIN)
digitalWrite(STNDBY_PIN, HIGH);
#endif
digitalWrite(LED_PIN, HIGH);
} else {
#if defined(STNDBY_PIN)
digitalWrite(STNDBY_PIN, LOW);
#endif
digitalWrite(LED_PIN, LOW);
}
}
return val;
}
class StreamDecoder {
private:
int16_t begin_x, begin_y, x, y;
const char *data_top, *data_ptr, *data_end;
uint8_t delta_bits;
uint8_t delta_shft;
public:
StreamDecoder(const char *data, size_t siz) {
data_top = data_ptr = data;
data_end = data_top + siz;
reset();
}
void reset() {
data_ptr = data_top;
x = begin_x = (pgm_read_byte(data_ptr++) << 8) | pgm_read_byte(data_ptr++);
y = begin_y = (pgm_read_byte(data_ptr++) << 8) | pgm_read_byte(data_ptr++);
delta_bits = 0;
delta_shft = 8;
}
bool get(int16_t &out_x, int16_t &out_y) {
out_x = x;
out_y = y;
// Data stream is over if we return to the starting point
// on the last delta byte.
if(data_ptr == data_end && begin_x == x && begin_y == y)
return false;
if(delta_shft == 8) {
delta_bits = pgm_read_byte(data_ptr++);
delta_shft = 0;
}
switch((delta_bits >> delta_shft) & 3) {
case 0: x++; break;
case 1: x--; break;
case 2: y++; break;
case 3: y--; break;
}
delta_shft += 2;
return true;
}
};
void dumpPoints(class StreamDecoder& s) {
delay(5000);
Serial.println("begin");
for(bool more = true; more;) {
int16_t x, y;
more = s.get(x,y);
Serial.print(x);
Serial.print(",");
Serial.println(y);
}
Serial.println("end");
s.reset();
}
/******************************* MAIN PROGRAM *******************************/
constexpr uint8_t fps = 14;
constexpr uint32_t us_per_pt = 1000000/fps/((sizeof(heart)-4)*4);
StreamDecoder stream(heart, sizeof(heart));
void setup() {
Serial.begin(9600);
setPWMTimerFrequencies(PWM_FREQ_31372Hz);
setDriverPins();
//dumpPoints(stream);
}
void loop() {
static uint32_t fps_millis = millis();
static uint8_t fps_frames = 0;
uint32_t us = micros();
uint8_t a = getMaxAmplitude();
if(a) {
int16_t x, y;
if(!stream.get(x,y)) {
stream.reset();
stream.get(x,y);
fps_frames++;
if(fps_frames == fps*2) {
const float elapsed_sec = float(millis() - fps_millis)/1000;
Serial.print("Actual FPS: ");
Serial.println(fps_frames / elapsed_sec);
fps_millis = millis();
fps_frames = 0;
}
}
setAmplitude(x, y, a);
}
uint32_t elapsed = micros() - us;
if(elapsed < us_per_pt)
delayMicroseconds(us_per_pt - elapsed);
}