Implemented freerunning ADC
and disabled PWM
This commit is contained in:
Michael Rest
@@ -19,7 +19,7 @@ void set_cursor (uint8_t x, uint8_t y);
|
||||
#define CLEAR_DISPLAY 0x01
|
||||
#define CURSOR_HOME 0x02
|
||||
//
|
||||
// Pinbelegung für das LCD, an verwendete Pins anpassen
|
||||
// Pinbelegung fuer das LCD, an verwendete Pins anpassen
|
||||
|
||||
#ifndef LCD_PORT
|
||||
#define LCD_PORT PORTC
|
||||
|
||||
168
screwer.c
168
screwer.c
@@ -24,23 +24,19 @@ nst, 07.11.2005
|
||||
#define LCD_EN PC6
|
||||
|
||||
//Devel
|
||||
//#define CURRENT_RAW
|
||||
//#define TORQUE_DEBUG
|
||||
//#define ADC_DEBUG
|
||||
#define TORQUE_DEBUG //To debug floating AVG
|
||||
//#define ADC_DEBUG //To debug ADC channels
|
||||
|
||||
//Prototypes
|
||||
void pwm_init (void);
|
||||
void tc0_init (void);
|
||||
void ADC_init (void);
|
||||
uint16_t ReadADC (uint8_t mux);
|
||||
|
||||
|
||||
//global Vars for ISR Access
|
||||
volatile unsigned char gcStopped;
|
||||
volatile uint16_t gu16SpeedDelay;
|
||||
volatile uint16_t gu16Test;
|
||||
volatile double gfSetSpeed;
|
||||
volatile uint16_t gu16AdcRes[USED_ADC_CHANNELS];
|
||||
volatile tFloatAvgFilter FilterCurrent;
|
||||
volatile tFloatAvgFilter gFilterCurrent;
|
||||
|
||||
|
||||
//ISR Timer 0 - for future usage
|
||||
@@ -50,15 +46,6 @@ ISR (TIMER0_OVF_vect)
|
||||
}
|
||||
|
||||
|
||||
//ISR Timer 1 - measure current on Bottom of PWM
|
||||
ISR (TIMER1_OVF_vect)
|
||||
{
|
||||
//uint16_t u16Speed = (uint16_t)(gfSetSpeed / (double) gu16AdcRes [ADC_CURRENT]);
|
||||
//AddToFloatAvg (&FilterCurrent, gu16AdcRes [ADC_CURRENT] / gu16Test);
|
||||
gu16Test = gu16AdcRes [ADC_CURRENT];
|
||||
}
|
||||
|
||||
|
||||
//ISR ADC Converision complete
|
||||
ISR (ADC_vect)
|
||||
{
|
||||
@@ -75,19 +62,12 @@ ISR (ADC_vect)
|
||||
{
|
||||
gu16AdcRes [ADC_CURRENT] = 512 - gu16AdcRes [ADC_CURRENT];
|
||||
}
|
||||
AddToFloatAvg (&gFilterCurrent, gu16AdcRes [ADC_CURRENT]);
|
||||
}
|
||||
|
||||
u8ActChannel += 1;
|
||||
if (gcStopped)
|
||||
{
|
||||
if (u8ActChannel == 3)
|
||||
if (u8ActChannel == 3)
|
||||
u8ActChannel = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (u8ActChannel == 2)
|
||||
u8ActChannel = 0;
|
||||
}
|
||||
ADMUX = (ADMUX & 0xE0) | u8ActChannel;
|
||||
}
|
||||
|
||||
@@ -99,10 +79,10 @@ int main (void)
|
||||
char sBuffer [20];
|
||||
char sState [3];
|
||||
unsigned char bStarted, bMotorLeft, bTorqueReached = 0;
|
||||
uint16_t uiCurrent, uiCurrentMin, uiCurrentMax, uiCurrentReached, u16SetSpeed, uiSetpointSpeed = 0, u16LCDTrigger = 1;
|
||||
double fTorque, fSetpointTorque, fReachedTorque;
|
||||
uint16_t u16Current, u16CurrentZero, u16CurrentReached, u16CurrentReachedRAW;
|
||||
uint16_t u16SetpointSpeed = 0, u16LCDTrigger = 1;
|
||||
double fTorque, fSetpointTorque, fReachedTorque, fReachedTorqueRAW;
|
||||
|
||||
uint16_t uiCurrentZero = 100;
|
||||
const double fCurrentStep = 0.02466;
|
||||
const double fTorqueConstant = 0.630; //Nm/A from Motortype
|
||||
const double fTorqueSetpointStep = 0.00615234; //From Sensortype 0.009885
|
||||
@@ -115,26 +95,31 @@ int main (void)
|
||||
DDRB = 0x00; // Set Port Read
|
||||
PORTB = 0xff; //Activate Pullups
|
||||
|
||||
|
||||
//Lock Low Side of H-Bride !!!!
|
||||
PORTD = (1 << PD6) | (1 << PD7);
|
||||
DDRD = (1 << PD0) | (1 << PD4) | (1 << PD5) | (1 << PD6) | (1 << PD7) ; //Set PBD5 (OC1A) as Write
|
||||
|
||||
//Enable Interrupts
|
||||
sei ();
|
||||
|
||||
//Init PWM
|
||||
pwm_init ();
|
||||
|
||||
//Set Timer 0
|
||||
//tc0_init ();
|
||||
|
||||
//ADC initialisieren
|
||||
ADC_init ();
|
||||
|
||||
// initialisieren und mit aktuellem wert fuellen
|
||||
uiCurrentZero = gu16AdcRes [ADC_CURRENT];
|
||||
InitFloatAvg (&FilterCurrent, uiCurrentZero);
|
||||
InitFloatAvg (&gFilterCurrent, gu16AdcRes [ADC_CURRENT]);
|
||||
|
||||
|
||||
// Disclaimer
|
||||
set_cursor (0, 1);
|
||||
lcd_string (" MS-7DA v1.00");
|
||||
set_cursor (0, 2);
|
||||
lcd_string (" Babacek Rest");
|
||||
for (u16LCDTrigger = 80000; u16LCDTrigger > 0; u16LCDTrigger--) _delay_ms (1);
|
||||
|
||||
//Calibrate
|
||||
u16CurrentZero = GetOutputValue (&gFilterCurrent);
|
||||
|
||||
//LDC Light ON
|
||||
LCD_PORT|= (1 << LCD_LED);
|
||||
@@ -144,36 +129,19 @@ int main (void)
|
||||
|
||||
//Read Current Vallue fom IC
|
||||
//Floating Average
|
||||
//uiCurrent = GetOutputValue (&FilterCurrent);
|
||||
uiCurrent = gu16AdcRes [ADC_CURRENT];
|
||||
u16Current = GetOutputValue (&gFilterCurrent);
|
||||
//uiCurrent = gu16AdcRes [ADC_CURRENT];
|
||||
|
||||
//Calculate Speed
|
||||
u16SetSpeed = (uiSetpointSpeed - gu16SpeedDelay) >> 2; //TOP 0xff
|
||||
gu16Test = 0xff / u16SetSpeed ;
|
||||
//gfSetSpeed = (double) u16SetSpeed / 255.0;
|
||||
|
||||
|
||||
//Debug limits of Torque
|
||||
#ifdef TORQUE_DEBUG
|
||||
if (!gu16SpeedDelay)
|
||||
{
|
||||
if (gu16AdcRes [ADC_CURRENT] > uiCurrentMax)
|
||||
uiCurrentMax = gu16AdcRes [ADC_CURRENT];
|
||||
if (gu16AdcRes [ADC_CURRENT] < uiCurrentMin)
|
||||
uiCurrentMin = gu16AdcRes [ADC_CURRENT];
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
gcStopped = ((PIND & (1 << PD2)) == 0);
|
||||
if (gcStopped)
|
||||
{
|
||||
bMotorLeft = PIND & (1 << PD3);
|
||||
uiCurrentMax = 0;
|
||||
//Motor stopped so recalibrate
|
||||
//FixMe bad idea uiCurrentZero = uiCurrent;
|
||||
//FixMe bad idea u16CurrentZero = u16Current;
|
||||
//Get potis
|
||||
uiSetpointSpeed = gu16AdcRes [ADC_SETPOINT_SPEED];
|
||||
u16SetpointSpeed = gu16AdcRes [ADC_SETPOINT_SPEED];
|
||||
fSetpointTorque = (double) gu16AdcRes [ADC_SETPOINT_TORQUE] * fTorqueSetpointStep;
|
||||
//Minumum Torque
|
||||
if (fSetpointTorque < 0.5)
|
||||
@@ -189,23 +157,23 @@ int main (void)
|
||||
}
|
||||
|
||||
|
||||
if (uiCurrent < uiCurrentZero)
|
||||
uiCurrent = 0;
|
||||
if (u16Current < u16CurrentZero)
|
||||
u16Current = 0;
|
||||
else
|
||||
uiCurrent -= uiCurrentZero;
|
||||
u16Current -= u16CurrentZero;
|
||||
|
||||
fTorque = (double)(uiCurrent) * fCurrTorqueConst;
|
||||
fTorque = (double)(u16Current) * fCurrTorqueConst;
|
||||
|
||||
//Start Motor left
|
||||
if ((PIND & (1 << PD2)) && bMotorLeft && !bTorqueReached)
|
||||
{
|
||||
OCR1B = 0;
|
||||
|
||||
//Prepare Low Side
|
||||
PORTD |= (1 << PD7);
|
||||
PORTD &= ~(1 << PD6);
|
||||
////PORTD |= (1 << PD5);
|
||||
////PORTD &= ~(1 << PD4);
|
||||
OCR1A = u16SetSpeed;
|
||||
//Set High Side
|
||||
PORTD |= (1 << PD5);
|
||||
PORTD &= ~(1 << PD4);
|
||||
|
||||
if (!bStarted)
|
||||
{
|
||||
strcpy (sState, "Li");
|
||||
@@ -215,14 +183,12 @@ int main (void)
|
||||
//Start Motor right
|
||||
else if ((PIND & (1 << PD2)) && !bMotorLeft && !bTorqueReached)
|
||||
{
|
||||
|
||||
OCR1A = 0;
|
||||
|
||||
//Prepare Low Side
|
||||
PORTD |= (1 << PD6);
|
||||
PORTD &= ~(1 << PD7);
|
||||
//PORTD |= (1 << PD4);
|
||||
//PORTD &= ~(1 << PD5);
|
||||
OCR1B = u16SetSpeed;
|
||||
//Set High Side
|
||||
PORTD |= (1 << PD4);
|
||||
PORTD &= ~(1 << PD5);
|
||||
if (!bStarted)
|
||||
{
|
||||
strcpy (sState, "Re");
|
||||
@@ -231,12 +197,11 @@ int main (void)
|
||||
}
|
||||
else if (gcStopped)
|
||||
{
|
||||
//Stop - no operation
|
||||
OCR1A = 0;
|
||||
OCR1B = 0;
|
||||
PORTD &= ~(1 << PD4);
|
||||
PORTD &= ~(1 << PD5);
|
||||
//smooth stop
|
||||
PORTD |= (1 << PD6) | (1 << PD7);
|
||||
gu16SpeedDelay = uiSetpointSpeed;
|
||||
gu16SpeedDelay = u16SetpointSpeed;
|
||||
bTorqueReached = 0;
|
||||
bStarted = 0;
|
||||
strcpy (sState, "St");
|
||||
@@ -245,21 +210,25 @@ int main (void)
|
||||
if ((fTorque > fSetpointTorque) && !gu16SpeedDelay && !bTorqueReached)
|
||||
{
|
||||
//Torque Stop after start ramp
|
||||
OCR1A = 0x00;
|
||||
OCR1B = 0x00;
|
||||
PORTD &= ~(1 << PD4);
|
||||
PORTD &= ~(1 << PD5);
|
||||
//smooth stop
|
||||
PORTD |= (1 << PD6) | (1 << PD7);
|
||||
//Fast Stop
|
||||
//PORTD &= ~(1 << PD6);
|
||||
//PORTD &= ~(1 << PD7);
|
||||
|
||||
bTorqueReached = 1;
|
||||
u16CurrentReached = u16Current;
|
||||
u16CurrentReachedRAW = gu16AdcRes [ADC_CURRENT];
|
||||
fReachedTorque = fTorque;
|
||||
uiCurrentReached = uiCurrent;
|
||||
fReachedTorqueRAW = (double)(u16CurrentReachedRAW) * fCurrTorqueConst;
|
||||
strcpy (sState, "ST");
|
||||
}
|
||||
|
||||
//LCD
|
||||
u16LCDTrigger ++;
|
||||
if (u16LCDTrigger == 2500)
|
||||
if (u16LCDTrigger == 5000)
|
||||
{
|
||||
#ifdef ADC_DEBUG
|
||||
set_cursor (0, 1);
|
||||
@@ -267,12 +236,18 @@ int main (void)
|
||||
lcd_string (sBuffer);
|
||||
#else
|
||||
if (gcStopped)
|
||||
{
|
||||
set_cursor (0, 1);
|
||||
sprintf (sBuffer, "v=%3d%% M=%03.1fNm", ((u16SetpointSpeed - (u16SetpointSpeed /1000) * u16SetpointSpeed %1000)/ 10), fSetpointTorque);
|
||||
lcd_string (sBuffer);
|
||||
}
|
||||
if (bTorqueReached)
|
||||
{
|
||||
set_cursor (0, 1);
|
||||
#ifdef TORQUE_DEBUG
|
||||
sprintf (sBuffer, "m%d M%d", uiCurrentMin, uiCurrentMax);
|
||||
sprintf (sBuffer, "%s I%dA M%04.2fN", sState, u16CurrentReachedRAW, fReachedTorqueRAW);
|
||||
#else
|
||||
sprintf (sBuffer, "v=%3d%% M=%03.1fNm", ((uiSetpointSpeed - (uiSetpointSpeed /1000) * uiSetpointSpeed %1000)/ 10), fSetpointTorque);
|
||||
sprintf (sBuffer, "M erreicht);
|
||||
#endif
|
||||
lcd_string (sBuffer);
|
||||
}
|
||||
@@ -282,10 +257,10 @@ int main (void)
|
||||
if (bTorqueReached)
|
||||
{
|
||||
sprintf (sBuffer, "%s M=%03.2fNm ", sState, fReachedTorque);
|
||||
sprintf (sBuffer, "%s I%dA M%04.2fN", sState, uiCurrentReached, fReachedTorque);
|
||||
sprintf (sBuffer, "%s I%dA M%04.2fN", sState, u16CurrentReached, fReachedTorque);
|
||||
}
|
||||
else
|
||||
sprintf (sBuffer, "%s M=%04.2f I=%d ", sState, fTorque, uiCurrent);
|
||||
sprintf (sBuffer, "%s M=%04.2f I=%d ", sState, fTorque, u16Current);
|
||||
|
||||
lcd_string (sBuffer);
|
||||
u16LCDTrigger = 0;
|
||||
@@ -295,28 +270,8 @@ int main (void)
|
||||
|
||||
}
|
||||
|
||||
/*==========================================*/
|
||||
void pwm_init (void)
|
||||
{
|
||||
//Compare values to zero
|
||||
OCR1A = 0x0;
|
||||
OCR1B = 0x0;
|
||||
// Set timer 1
|
||||
// OC1A/OC1B clear on Match, Set on Top
|
||||
// 8 bit resolution , Phase Correct PWM
|
||||
TCCR1A = (1 << COM1A1) | (0 << COM1A0) | \
|
||||
(1 << COM1B1) | (0 << COM1B0) | \
|
||||
(0 << WGM13) | (0 << WGM12) | (0 << WGM11) | (1 << WGM10);
|
||||
// Pre divisor of Timer 1
|
||||
TCCR1B = (0 << CS12) | (0 << CS11) | (1 << CS10);
|
||||
|
||||
//Enable overflow interrupt
|
||||
TIMSK |= (1 << TOIE1);
|
||||
|
||||
|
||||
//Set TOP value
|
||||
ICR1 = 0xFF;
|
||||
}
|
||||
|
||||
|
||||
/*==========================================*/
|
||||
@@ -328,8 +283,7 @@ void tc0_init (void)
|
||||
(0 << CS02) | (1 << CS01) | (0 << CS00);
|
||||
|
||||
//Enable overflow interrupt
|
||||
//TIMSK |= (1 << TOIE0);
|
||||
|
||||
TIMSK |= (1 << TOIE0);
|
||||
}
|
||||
|
||||
|
||||
@@ -346,5 +300,5 @@ void ADC_init (void)
|
||||
ADCSRA = (1 << ADEN) | (1 << ADIE) | (1 << ADATE)| (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
|
||||
ADCSRA |= (1 << ADSC);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user