#define DIGITAL_WRITE_EXPENSIVE_INLINE_OPTIMIZATION
#include <avr/interrupt.h>
#include <util/atomic.h>
#include <math.h>
#include <string.h>
#include "WConstants.h"
#include "core_pins.h"
#include "J12Driver20t2pp.h"
#include "usb_api.h"
/******************************************************************************
* Definitions
******************************************************************************/
#define TIMER_PULSE_PER_SEC 7600UL // Set pulse to 4.0KHz
#define J12_MICRO_STEP_PER_DEGREE (12.0)
// #define J12_MICRO_STEP_PER_DEGREE (1)
//#define J12_STEP_SPEED_COUNT_SUPER_SLOW 100
//#define J12_STEP_SPEED_COUNT_SLOW 8
//#define J12_STEP_SPEED_COUNT_MEDIUM_SLOW 4
//#define J12_STEP_SPEED_COUNT_MEDIUM_FAST 2
//#define J12_STEP_SPEED_COUNT_FAST 1
#define J12_MOTOR_DIRECTION_CCW LOW // counter-clockwise
#define J12_MOTOR_DIRECTION_CW HIGH // clockwise
#define PIO_J12_DRIVER1_XRESET PIN_E4
#define PIO_J12_DRIVER1_MSEL PIN_E4
#define PIO_J12_DRIVER2_XRESET PIN_E4
#define PIO_J12_DRIVER2_MSEL PIN_E4
/******************************************************************************
* Globals
******************************************************************************/
static const j12_pio_t g_j12_pio[J12_DRIVER_IDX_MAX] = {
{PIN_D0, PIN_B7, PIN_F5}, // Oil P // FSCX, CW, CAL
{PIN_D2, PIN_D1, PIN_F4}, //ROC
{PIN_D4, PIN_D3, PIN_F6}, //ASI
{PIN_D6, PIN_D5, PIN_F5}, //amps
{PIN_E0, PIN_D7, PIN_F5}, // rpm R
{PIN_C0, PIN_E1, PIN_F3}, // MP
{PIN_C2, PIN_C1, PIN_F5}, // oil T
{PIN_C4, PIN_C3, PIN_F5}, // RPM E
{PIN_C6, PIN_C5, PIN_F5}, // motor 9 je Compass
{PIN_A2, PIN_C7, PIN_F5}, // motor 10 alt a
{PIN_A7, PIN_A3, PIN_F5}, // motor 11 alt b
{PIN_F7, PIN_A6, PIN_F5}, // motor 12 alt c
{PIN_E6, PIN_F0, PIN_F5}, // motor 13 Carb Temp
{PIN_B0, PIN_E7, PIN_F5}, // motor 14NAv1obs
{PIN_B2, PIN_B1, PIN_F5}, // motor 15 CHT
{PIN_F1, PIN_F2, PIN_F5}, // motor 16 FUEL MAIN
{PIN_B6, PIN_B5, PIN_F5}, // motor 17 FUEL AUX
{PIN_E7, PIN_B0, PIN_F5} // not working on R22 YP
};
static j12_driver_t g_j12Driver[J12_DRIVER_IDX_MAX];
static const j12_speed_t g_j12_speed[J12_SPEED_PROFILE_MAX] =
{
{{{1*J12_MICRO_STEP_PER_DEGREE, 8}, // angle, speed count
{4*J12_MICRO_STEP_PER_DEGREE, 8},
{8*J12_MICRO_STEP_PER_DEGREE, 4},
{16*J12_MICRO_STEP_PER_DEGREE, 2},
{32*J12_MICRO_STEP_PER_DEGREE, 1}}},
{{{3*J12_MICRO_STEP_PER_DEGREE, 100},
{6*J12_MICRO_STEP_PER_DEGREE, 50}, //8
{10*J12_MICRO_STEP_PER_DEGREE, 25}, //4
{20*J12_MICRO_STEP_PER_DEGREE, 4},
{32*J12_MICRO_STEP_PER_DEGREE, 2}
}}};
/******************************************************************************
* User API
******************************************************************************/
J12DriverClass g_J12Driver;
void J12DriverClass::Init()
{
uint8_t i;
memset(&g_j12Driver, 0, sizeof(g_j12Driver));
for(i=0; i<J12_DRIVER_IDX_MAX; i++)
{
g_j12Driver[i].pio = &g_j12_pio[i];
// setup the PIO direction
pinMode(g_j12Driver[i].pio->pioFSCX, OUTPUT);
digitalWrite(g_j12Driver[i].pio->pioFSCX, LOW);
pinMode(g_j12Driver[i].pio->pioCW, OUTPUT);
digitalWrite(g_j12Driver[i].pio->pioCW, LOW);
pinMode(g_j12Driver[i].pio->pioCal, INPUT);
//digitalWrite(g_j12Driver[i].pio->pioCal, LOW); is this stopiing the cal signal?
// digitalWrite(g_j12Driver[i].pio->pioCal, LOW);
g_j12Driver[i].speed = &g_j12_speed[J12_SPEED_PROFILE1]; // default speed profile
}
m_pioXReset[0] = PIO_J12_DRIVER1_XRESET;
m_pioXReset[1] = PIO_J12_DRIVER2_XRESET;
for(i=0; i<NUM_J12_QUAD_DRIVER; i++)
{
// initialize and toggle the reset pins
pinMode(m_pioXReset[i], OUTPUT);
digitalWrite(m_pioXReset[i], HIGH); // je
delayMicroseconds(100); //je
digitalWrite(m_pioXReset[i], LOW);
delayMicroseconds(100); // wait 100us
digitalWrite(m_pioXReset[i], HIGH);
}
// initialize the pulse timer ISR
InitPulseTimer();
}
void J12DriverClass::InitPulseTimer()
{
ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
{
// initialize Timer1
TCCR1A = 0; // set entire TCCR1A register to 0
TCCR1B = 0; // same for TCCR1B
// set compare match register to desired timer count:
OCR1A = (F_CPU/TIMER_PULSE_PER_SEC)-1;
// turn on CTC mode:
TCCR1B |= _BV(WGM12);
// Set CS10 bit CS11 bit 0, CS12 bit is 0, so timer runs at clock speed:
TCCR1B |= _BV(CS10);
// enable timer compare interrupt:
TIMSK1 |= _BV(OCIE1A);
}
}
void J12DriverClass::Calibrate(j12_driver_idx_e driverIdx)
{
float angle = 0;
uint8_t ledCal;
uint8_t loop_count = 0;
// set the motor to zero with a reset
g_j12Driver[driverIdx].microStep = 0;
while((digitalRead(g_j12Driver[driverIdx].pio->pioCal) == LOW) &&
(loop_count < 3))
{
angle = GetAngle(driverIdx);
angle = angle + 0.10;
SetAngle(driverIdx, angle, true, false);
delayMicroseconds(150); // wait 150us
if(angle >= 360)
{
angle = 0;
loop_count++;
}
}
loop_count = 0;
while((digitalRead(g_j12Driver[driverIdx].pio->pioCal) == HIGH) &&
(loop_count < 3))
{
angle = GetAngle(driverIdx);
angle = angle + 0.10;
SetAngle(driverIdx, angle, true, false);
delayMicroseconds(150); // wait 150us
if(angle >= 360)
{
angle = 0;
loop_count++;
}
}
// set the angle to zero (micro step zero)
g_j12Driver[driverIdx].microStep = 0;
}
/* je Calibration t6 is for use with the bottom 6 instrument panel . External stop.*/
void J12DriverClass::Calibrate_t6(j12_driver_idx_e driverIdx)
{
float angle = 0;
uint8_t ledCal;
uint8_t loop_count = 0;
// set the angle to centre (micro step zero)
g_j12Driver[driverIdx].microStep = 2160;
SetAngle(driverIdx, 0, true, false);
delay (1000);
SetAngle(driverIdx, 44, true, false);
delay (100);
g_j12Driver[driverIdx].microStep = 528; // je puts pointer referenced to dot on gauge
}
void J12DriverClass::Calibrate_tachs(j12_driver_idx_e driverIdx)
{
float angle = 0;
uint8_t ledCal;
uint8_t loop_count = 0;
// set the angle to centre (micro step zero)
g_j12Driver[driverIdx].microStep = 3600; // je use pillar to move pointer to external stop
SetAngle(driverIdx, 0, false, false);
delay (1000);
SetAngle(driverIdx, 40, false, false); // place pointer on 46%
delay (100);
g_j12Driver[driverIdx].microStep = 0; // make this 0 degrees
}
// je calibrate engine rpm reversed motor !!
void J12DriverClass::Calibrate_tachs_e(j12_driver_idx_e driverIdx)
{
float angle = 0;
uint8_t ledCal;
uint8_t loop_count = 0;
// set the angle to centre (micro step zero)
g_j12Driver[driverIdx].microStep = 3600; // je use pillar to move ointer to external stop
SetAngle(driverIdx, 0, false, false);
delay (1000);
SetAngle(driverIdx, 164, false, false); // place pointer on 46%
delay (100);
g_j12Driver[driverIdx].microStep = 0; // make this 0 degrees
}
void J12DriverClass::Calibrate_amps(j12_driver_idx_e driverIdx)
{
float angle = 0;
uint8_t ledCal;
uint8_t loop_count = 0;
// set the angle to centre (micro step zero)
g_j12Driver[driverIdx].microStep = 2160;
SetAngle(driverIdx, 0, true, false);
delay (1000);
SetAngle(driverIdx, 90, true, false);
}
void J12DriverClass::Calibrate_carbtemp(j12_driver_idx_e driverIdx)
{
float angle = 0;
uint8_t ledCal;
uint8_t loop_count = 0;
// set the angle to centre (micro step zero)
g_j12Driver[driverIdx].microStep = 2160;
SetAngle(driverIdx, 0, true, false);
delay (1000);
SetAngle(driverIdx, 90, true, false);
}
void J12DriverClass::SetStepProfile(j12_driver_idx_e driverIdx, j12_speed_profile_e profile_type)
{
g_j12Driver[driverIdx].speed = &g_j12_speed[profile_type]; // default speed profile
}
void J12DriverClass::SetAngle(j12_driver_idx_e driverIdx, float angle, bool wrap)
{
SetAngle(driverIdx, angle, wrap, true);
}
/*
void J12DriverClass::Calibrate_amps(j12_driver_idx_e driverIdx)
{
float angle = 0;
uint8_t ledCal;
uint8_t loop_count = 0;
g_j12Driver[driverIdx].microStep = 2160;
SetAngle(driverIdx, 0, true, false);
// set the angle to zero (micro step zero)
g_j12Driver[driverIdx].microStep = 0;
SetAngle(driverIdx, 90, true, false);
delay (1000);
}
*/
// set the angle for the motor driver
void J12DriverClass::SetAngle(j12_driver_idx_e driverIdx, float angle, bool wrap, bool smoothing)
{
int16_t newMicroStep;
//uint8_t last_direction;
// convert the float angle to 1/12th degree ticks
newMicroStep = (angle * J12_MICRO_STEP_PER_DEGREE) + 0.5;
// first set the pulsecount to zero to stop
// the current pulse train, so the angle will
// not change.
ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
{
g_j12Driver[driverIdx].pulseCount = 0; // stop pulse counts
g_j12Driver[driverIdx].pulseState = LOW;
//last_direction = g_j12Driver[driverIdx].direction;
if(g_j12Driver[driverIdx].microStep >= (360 * J12_MICRO_STEP_PER_DEGREE))
g_j12Driver[driverIdx].microStep -= (360 * J12_MICRO_STEP_PER_DEGREE);
else if(g_j12Driver[driverIdx].microStep < 0)
g_j12Driver[driverIdx].microStep += (360 * J12_MICRO_STEP_PER_DEGREE);
}
delayMicroseconds(4); // wait 4us
digitalWrite(g_j12Driver[driverIdx].pio->pioFSCX, LOW); // set the pulse low
if(true == wrap)
{
int16_t deltaMicroStep;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
{
deltaMicroStep = g_j12Driver[driverIdx].microStep - newMicroStep;
}
if(deltaMicroStep < 0)
{
deltaMicroStep += (360 * J12_MICRO_STEP_PER_DEGREE);
}
if(deltaMicroStep > (180 * J12_MICRO_STEP_PER_DEGREE))
{
// set motor direction clockwise
g_j12Driver[driverIdx].direction = J12_MOTOR_DIRECTION_CW;
digitalWrite(g_j12Driver[driverIdx].pio->pioCW, J12_MOTOR_DIRECTION_CW);
// correct the update angle
deltaMicroStep = (360 * J12_MICRO_STEP_PER_DEGREE) - deltaMicroStep;
}
else
{
// set motor direction counter clockwise
g_j12Driver[driverIdx].direction = J12_MOTOR_DIRECTION_CCW;
digitalWrite(g_j12Driver[driverIdx].pio->pioCW, J12_MOTOR_DIRECTION_CCW);
}
// make this an atomic operation
ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
{
g_j12Driver[driverIdx].pulseCount = deltaMicroStep;
g_j12Driver[driverIdx].enable_smoothing = smoothing;
g_j12Driver[driverIdx].pulse_speed_count = 0;
if(g_j12Driver[driverIdx].pulseCount < g_j12Driver[driverIdx].speed->step[J12_SPEED_STEP_SUPER_SLOW].angle)
{
g_j12Driver[driverIdx].pulse_speed_count = g_j12Driver[driverIdx].speed->step[J12_SPEED_STEP_SUPER_SLOW].count;
}
}
//Serial.println(g_j12Driver[driverIdx].pulseCount);
}
else
{
// calculate how much to change the motor angle
// form the current angle and its direction.
if(newMicroStep > g_j12Driver[driverIdx].microStep)
{
g_j12Driver[driverIdx].direction = J12_MOTOR_DIRECTION_CW;
digitalWrite(g_j12Driver[driverIdx].pio->pioCW, J12_MOTOR_DIRECTION_CW);
}
else
{
g_j12Driver[driverIdx].direction = J12_MOTOR_DIRECTION_CCW;
digitalWrite(g_j12Driver[driverIdx].pio->pioCW, J12_MOTOR_DIRECTION_CCW);
}
// make this an atomic operation
ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
{
g_j12Driver[driverIdx].pulseCount = abs(g_j12Driver[driverIdx].microStep - newMicroStep);
g_j12Driver[driverIdx].enable_smoothing = smoothing;
g_j12Driver[driverIdx].pulse_speed_count = 0;
if(g_j12Driver[driverIdx].pulseCount < g_j12Driver[driverIdx].speed->step[J12_SPEED_STEP_SUPER_SLOW].angle)
{
g_j12Driver[driverIdx].pulse_speed_count = g_j12Driver[driverIdx].speed->step[J12_SPEED_STEP_SUPER_SLOW].count;
}
}
//Serial.println(g_j12Driver[driverIdx].pulseCount);
}
}
// get the current angle of the gauge driver
float J12DriverClass::GetAngle(j12_driver_idx_e driverIdx)
{
float angle;
// convert the 1/12th thick degree angle to a float.
ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
{
angle = g_j12Driver[driverIdx].microStep;
}
angle /= J12_MICRO_STEP_PER_DEGREE;
if(angle >= 360)
angle -= 360;
else if(angle < 0)
angle += 360;
return(angle);
}
// create the pulse train ISR
// alternate down/up pulse
// on the up pulse look at the pulse count if not 0 set a pulse
// decrement the pulseCount and increment/decrement the microStep
// depending on the direction set.
ISR(TIMER1_COMPA_vect)
{
uint8_t i, j;
for(i=0; i<J12_DRIVER_IDX_MAX; i++)
{
if(g_j12Driver[i].pulseCount > 0)
{
if((true == g_j12Driver[i].enable_smoothing) &&
(g_j12Driver[i].pulse_speed_count > 0))
{
g_j12Driver[i].pulse_speed_count--;
if(g_j12Driver[i].pulseCount > 500)
{
g_j12Driver[i].pulse_speed_count = 0;
}
}
else
{
if(LOW == g_j12Driver[i].pulseState)
{
// send one pulse to move the motor by 1/12th degree
digitalWrite(g_j12Driver[i].pio->pioFSCX, HIGH);
g_j12Driver[i].pulseState = HIGH;
g_j12Driver[i].pulseCount--;
// increment or decrement the micro step angle
if(J12_MOTOR_DIRECTION_CW == g_j12Driver[i].direction)
{
g_j12Driver[i].microStep++;
}
else
{
g_j12Driver[i].microStep--;
}
// check if micro step out of bounds
// might need to re-calibrate
}
else
{
digitalWrite(g_j12Driver[i].pio->pioFSCX, LOW);
g_j12Driver[i].pulseState = LOW;
}
// check at what speed to rotate the motor/gauge
for(j=0; j<J12_SPEED_STEP_MAX; j++)
{
if(g_j12Driver[i].pulseCount < g_j12Driver[i].speed->step[j].angle)
{
g_j12Driver[i].pulse_speed_count = g_j12Driver[i].speed->step[j].count;
#if 0
if(j > 0)
{
int delta_angle = g_j12Driver[i].speed->step[j].angle - g_j12Driver[i].speed->step[j-1].angle;
int pulse_angle = (float)g_j12Driver[i].pulseCount - g_j12Driver[i].speed->step[j-1].angle;
int pulse_count = g_j12Driver[i].speed->step[j-1].count - g_j12Driver[i].speed->step[j].count;
g_j12Driver[i].pulse_speed_count = g_j12Driver[i].speed->step[j-1].count - ((pulse_count * pulse_angle)/delta_angle);
}
else
{
g_j12Driver[i].pulse_speed_count = g_j12Driver[i].speed->step[j].count;
}
#endif
break;
}
}
}
}
}
}