Initial Commit

firmware/Makefile

@@ -0,0 +1,103 @@
+# Name: Makefile
+# Author: Michael Rest
+# Copyright: Michael Rest
+# License: Michael Rest
+
+# This is a prototype Makefile. Modify it according to your needs.
+# You should at least check the settings for
+# DEVICE ....... The AVR device you compile for
+# CLOCK ........ Target AVR clock rate in Hertz
+# OBJECTS ...... The object files created from your source files. This list is
+#                usually the same as the list of source files with suffix ".o".
+# PROGRAMMER ... Options to avrdude which define the hardware you use for
+#                uploading to the AVR and the interface where this hardware
+#                is connected. We recommend that you leave it undefined and
+#                add settings like this to your ~/.avrduderc file:
+#                   default_programmer = "stk500v2"
+#                   default_serial = "avrdoper"
+# FUSES ........ Parameters for avrdude to flash the fuses appropriately.
+
+DEVICE     = atmega644
+CLOCK      = 20000000
+PROGRAMMER = -c stk500v2 -P /dev/cu.usbserial-FTDEMYQO #avrdoper
+OBJECTS    = main.o floating_avg.o lcd-routines.o 
+#FUSES      = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
+FUSES      = -U hfuse:w:0x9f:m -U lfuse:w:0xff:m
+
+# ATMega8 fuse bits used above (fuse bits for other devices are different!):
+# Example for 8 MHz internal oscillator
+# Fuse high byte:
+# 0xd9 = 1 1 0 1   1 0 0 1 <-- BOOTRST (boot reset vector at 0x0000)
+#        ^ ^ ^ ^   ^ ^ ^------ BOOTSZ0
+#        | | | |   | +-------- BOOTSZ1
+#        | | | |   +---------- EESAVE (set to 0 to preserve EEPROM over chip erase)
+#        | | | +-------------- CKOPT (clock option, depends on oscillator type)
+#        | | +---------------- SPIEN (if set to 1, serial programming is disabled)
+#        | +------------------ WDTON (if set to 0, watchdog is always on)
+#        +-------------------- RSTDISBL (if set to 0, RESET pin is disabled)
+# Fuse low byte:
+# 0x24 = 0 0 1 0   0 1 0 0
+#        ^ ^ \ /   \--+--/
+#        | |  |       +------- CKSEL 3..0 (8M internal RC)
+#        | |  +--------------- SUT 1..0 (slowly rising power)
+#        | +------------------ BODEN (if 0, brown-out detector is enabled)
+#        +-------------------- BODLEVEL (if 0: 4V, if 1: 2.7V)
+#
+# For computing fuse byte values for other devices and options see
+# the fuse bit calculator at http://www.engbedded.com/fusecalc/
+
+
+# Tune the lines below only if you know what you are doing:
+
+AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE)
+COMPILE = avr-gcc -Wall -Os -std=gnu99 -DF_CPU=$(CLOCK) -mmcu=$(DEVICE)
+
+# symbolic targets:
+all:	main.hex
+
+.c.o:
+	$(COMPILE) -c $< -o $@
+
+.S.o:
+	$(COMPILE) -x assembler-with-cpp -c $< -o $@
+# "-x assembler-with-cpp" should not be necessary since this is the default
+# file type for the .S (with capital S) extension. However, upper case
+# characters are not always preserved on Windows. To ensure WinAVR
+# compatibility define the file type manually.
+
+.c.s:
+	$(COMPILE) -S $< -o $@
+
+flash:	all
+	$(AVRDUDE) -U flash:w:main.hex:i
+
+fuse:
+	$(AVRDUDE) $(FUSES)
+
+# Xcode uses the Makefile targets "", "clean" and "install"
+install: flash fuse
+
+# if you use a bootloader, change the command below appropriately:
+load: all
+	bootloadHID main.hex
+
+clean:
+	rm -f main.hex main.elf $(OBJECTS)
+
+# file targets:
+main.elf: $(OBJECTS)
+	$(COMPILE) -o main.elf $(OBJECTS)
+
+main.hex: main.elf
+	rm -f main.hex
+	avr-objcopy -j .text -j .data -O ihex main.elf main.hex
+	avr-size --format=avr --mcu=$(DEVICE) main.elf
+# If you have an EEPROM section, you must also create a hex file for the
+# EEPROM and add it to the "flash" target.
+
+# Targets for code debugging and analysis:
+disasm:	main.elf
+	avr-objdump -d main.elf
+
+cpp:
+	$(COMPILE) -E main.c

firmware/floating_avg.c

@@ -0,0 +1,39 @@
+#include "floating_avg.h"
+
+
+void InitFloatAvg (tFloatAvgFilter * io_pFloatAvgFilter, tFloatAvgType i_DefaultValue)
+{ 
+  gFloatAvgSum = 0;
+  // Init Buffer with initValue
+  for (uint8_t i = 0; i < (1 << SIZE_OF_AVG); ++i)
+  {
+    io_pFloatAvgFilter->aData[i] = i_DefaultValue;
+    gFloatAvgSum += i_DefaultValue;
+  }
+  io_pFloatAvgFilter->IndexNextValue = 0;
+} 
+
+
+void AddToFloatAvg(tFloatAvgFilter * io_pFloatAvgFilter, tFloatAvgType i_NewValue)
+{ 
+  // 1st remove oldest Value from Sum and than add new and store it
+  gFloatAvgSum -= io_pFloatAvgFilter->aData[io_pFloatAvgFilter->IndexNextValue];
+  gFloatAvgSum += i_NewValue;
+  io_pFloatAvgFilter->aData[io_pFloatAvgFilter->IndexNextValue] = i_NewValue;
+  io_pFloatAvgFilter->IndexNextValue++;
+  if (io_pFloatAvgFilter->IndexNextValue == SIZE_OF_AVG)
+    io_pFloatAvgFilter->IndexNextValue = 0;  
+  //io_pFloatAvgFilter->IndexNextValue %= (1 << SIZE_OF_AVG);
+}  
+
+
+tFloatAvgType GetOutputValue(tFloatAvgFilter * io_pFloatAvgFilter)
+{
+  tFloatAvgType o_Result = (tFloatAvgType) (gFloatAvgSum >> SIZE_OF_AVG);
+  return o_Result;
+}
+
+tFloatAvgType GetSum(tFloatAvgFilter * io_pFloatAvgFilter)
+{
+  return gFloatAvgSum;
+}

firmware/floating_avg.h

@@ -0,0 +1,43 @@
+#ifndef FLOATING_AVERAGE_H
+#define FLOATING_AVERAGE_H
+
+#include <inttypes.h>
+
+// Ueber wieviele Werte soll der gleitende Mittelwert berechnet werden?
+// 2**n Werte, da die Division als shift implementiert wird
+#define SIZE_OF_AVG  2
+
+// Datentyp, ueber den der gleitende Mittelwert berechnet werden soll.
+typedef uint16_t tFloatAvgType;
+// typedef float tFloatAvgType;
+
+// Wird nur intern fuer die Durchschnittsberechnung benutzt.
+// Muss Zahlen fassen koennen, die SIZE_OF_AVG mal groesser als tFloatAvgType sind.
+typedef uint32_t tTempSumType;
+// typedef float tTempSumType;
+
+
+
+// Die Struktur, in der die Daten zwischengespeichert werden
+typedef struct
+{
+  tFloatAvgType aData[1 << SIZE_OF_AVG];
+  uint8_t IndexNextValue;
+} tFloatAvgFilter;
+
+volatile tTempSumType gFloatAvgSum;
+
+
+// Initialisiert das Filter mit einem Startwert.
+void InitFloatAvg(tFloatAvgFilter * io_pFloatAvgFilter,
+    tFloatAvgType i_DefaultValue);
+
+// Schreibt einen neuen Wert in das Filter.
+void AddToFloatAvg(tFloatAvgFilter * io_pFloatAvgFilter,
+    tFloatAvgType i_ui16NewValue);
+
+// Berechnet den Durchschnitt aus den letzten SIZE_OF_AVG eingetragenen Werten.
+tFloatAvgType GetOutputValue(tFloatAvgFilter * io_pFloatAvgFilter);
+
+tFloatAvgType GetSum(tFloatAvgFilter * io_pFloatAvgFilter);
+#endif

firmware/lcd-routines.c

@@ -0,0 +1,150 @@
+// Ansteuerung eines HD44780 kompatiblen LCD im 4-Bit-Interfacemodus
+// http://www.mikrocontroller.net/articles/AVR-GCC-Tutorial/LCD-Ansteuerung
+//
+// Die Pinbelegung ist <20>ber defines in lcd-routines.h einstellbar
+ 
+#include <avr/io.h>
+#include "lcd-routines.h"
+#include <util/delay.h>
+ 
+// sendet ein Datenbyte an das LCD
+ 
+void lcd_data (unsigned char temp1)
+{
+   unsigned char temp2 = temp1;
+ 
+   LCD_PORT |=  (1 << LCD_RS);        // RS auf 1 setzen
+ 
+   temp1 = temp1 >> 4;
+   temp1 = temp1 & 0x0F;
+   LCD_PORTDATA &= 0xF0;
+   LCD_PORTDATA |= temp1;               // setzen
+   lcd_enable ();
+ 
+   temp2 = temp2 & 0x0F;
+   LCD_PORTDATA &= 0xF0;
+   LCD_PORTDATA |= temp2;               // setzen
+   lcd_enable ();
+   
+   _delay_us (42);
+}
+ 
+// sendet einen Befehl an das LCD
+ 
+void lcd_command (unsigned char temp1)
+{
+   unsigned char temp2 = temp1;
+ 
+   LCD_PORT &= ~(1 << LCD_RS);       // RS auf 0 setzen
+ 
+   temp1 = temp1 >> 4;              // oberes Nibble holen
+   temp1 = temp1 & 0x0F;            // maskieren
+   LCD_PORTDATA &= 0xF0;
+   LCD_PORTDATA |= temp1;               // setzen
+   lcd_enable ();
+ 
+   temp2 = temp2 & 0x0F;            // unteres Nibble holen und maskieren
+   LCD_PORTDATA &= 0xF0;
+   LCD_PORTDATA |= temp2;               // setzen
+   lcd_enable ();
+   
+   _delay_us (42);
+}
+ 
+// erzeugt den Enable-Puls
+void lcd_enable (void)
+{
+   // Bei Problemen ggf. Pause gem<65><6D> Datenblatt des LCD Controllers einf<6E>gen
+   // http://www.mikrocontroller.net/topic/81974#685882
+   LCD_PORT |= (1 << LCD_EN);
+   _delay_us (1);                   // kurze Pause
+   // Bei Problemen ggf. Pause gem<65><6D> Datenblatt des LCD Controllers verl<72>ngern
+   // http://www.mikrocontroller.net/topic/80900
+   LCD_PORT &= ~(1 << LCD_EN);
+}
+ 
+// Initialisierung: 
+// Muss ganz am Anfang des Programms aufgerufen werden.
+ 
+void lcd_init (void)
+{
+   LCD_DDR |= LCD_DDR | (1 << LCD_RW) | (1 << LCD_RS) | (1 << LCD_EN);   // Port auf Ausgang schalten
+   LCD_DDRDATA |= LCD_DDRDATA | 0x0F; // Port auf Ausgang schalten
+ 
+   // muss 3mal hintereinander gesendet werden zur Initialisierung
+ 
+   _delay_ms (15);
+   LCD_PORTDATA &= 0xF0;
+   LCD_PORTDATA |= 0x03;            
+   LCD_PORT &= ~(1 << LCD_RS);      // RS auf 0
+   LCD_PORT &= ~(1 << LCD_RW);      // R/W auf 0
+   lcd_enable ();
+ 
+   _delay_ms (5);
+   lcd_enable ();
+ 
+   _delay_ms (1);
+   lcd_enable ();
+   _delay_ms (1);
+ 
+   // 4 Bit Modus aktivieren 
+   LCD_PORTDATA &= 0xF0;
+   LCD_PORTDATA |= 0x02;
+   lcd_enable ();
+   _delay_ms (1);
+ 
+   // 4Bit / 2 Zeilen / 5x7
+   lcd_command (0x28);
+    
+   // Display ein / Cursor aus / kein Blinken
+   lcd_command (0x0C); 
+ 
+   // inkrement / kein Scrollen
+   lcd_command (0x06);
+ 
+   lcd_clear ();
+}
+ 
+// Sendet den Befehl zur L<>schung des Displays
+ 
+void lcd_clear (void)
+{
+   lcd_command (CLEAR_DISPLAY);
+   _delay_ms (5);
+}
+ 
+// Sendet den Befehl: Cursor Home
+ 
+void lcd_home (void)
+{
+   lcd_command (CURSOR_HOME);
+   _delay_ms (5);
+}
+ 
+// setzt den Cursor in Zeile y (1..4) Spalte x (0..15)
+ 
+void set_cursor (uint8_t x, uint8_t y)
+{
+  uint8_t tmp;
+ 
+  switch (y) 
+  {
+    case 1: tmp=0x80+0x00+x; break;    // 1. Zeile
+    case 2: tmp=0x80+0x40+x; break;    // 2. Zeile
+    case 3: tmp=0x80+0x10+x; break;    // 3. Zeile
+    case 4: tmp=0x80+0x50+x; break;    // 4. Zeile
+    default: return;                   // f<>r den Fall einer falschen Zeile
+  }
+  lcd_command (tmp);
+}
+ 
+// Schreibt einen String auf das LCD
+ 
+void lcd_string (char *data)
+{
+    while (*data) 
+    {
+       lcd_data (*data);
+       data++;
+    }
+}

firmware/lcd-routines.h

@@ -0,0 +1,33 @@
+// Ansteuerung eines HD44780 kompatiblen LCD im 4-Bit-Interfacemodus
+// http://www.mikrocontroller.net/articles/AVR-GCC-Tutorial/LCD-Ansteuerung
+//
+void lcd_data (unsigned char temp1);
+void lcd_string (char *data);
+void lcd_command (unsigned char temp1);
+void lcd_enable (void);
+void lcd_init (void);
+void lcd_home (void);
+void lcd_clear (void);
+void set_cursor (uint8_t x, uint8_t y);
+
+// Hier die verwendete Taktfrequenz in Hz eintragen, wichtig!
+
+#ifndef F_CPU
+ #define F_CPU 20000000
+#endif
+//    
+// LCD Befehle
+//     
+#define CLEAR_DISPLAY 0x01
+#define CURSOR_HOME   0x02
+//      
+// Pinbelegung fuer das LCD, an verwendete Pins anpassen
+//LCD Settings
+#define LCD_PORTDATA  PORTB
+#define LCD_LED       PB4
+#define LCD_DDRDATA   DDRB  //RW, RS, EN must be on same port
+#define LCD_PORT      PORTA
+#define LCD_DDR       DDRA  //RW, RS, EN must be on same port
+#define LCD_RW        PA5
+#define LCD_RS        PA6
+#define LCD_EN        PA7

firmware/main-ISR.c

@@ -0,0 +1,297 @@
+/*********************************************
+Screwdriver - Controller
+Compiler            : avr
+Chip type           : ATMEGA644
+MiR 2014
+*********************************************/
+#include <avr/io.h>
+#include <util/delay.h>
+#include <avr/interrupt.h>
+#include <string.h>
+#include <stdio.h>
+#include "lcd-routines.h"
+#include "floating_avg.h"
+
+
+//Constants
+#define USED_ADC_CHANNELS 4
+#define ADC_CURRENT 1
+#define ADC_CURRENT_ALT 2
+#define ADC_SETPOINT_SPEED 0
+#define ADC_SETPOINT_TORQUE 3
+
+//Devel
+//#define TORQUE_DEBUG //To debug floating AVG
+#define ADC_DEBUG //To debug ADC channels
+
+//Prototypes
+void  tc0_init (void);
+void  ADC_init (void);
+
+
+//global Vars for ISR Access
+volatile unsigned char gcStopped;
+volatile unsigned char gbStarted = 0;
+volatile uint16_t gu16SpeedDelay;
+volatile uint16_t gu16AdcRes[USED_ADC_CHANNELS];
+volatile tFloatAvgFilter gFilterCurrent;
+
+
+//ISR Timer 0 - for future usage
+ISR (TIMER0_OVF_vect)
+{
+
+}
+
+
+//ISR ADC Conversion complete
+ISR (ADC_vect)
+{
+  uint8_t u8ActChannel = (ADMUX & 0x1F);
+  gu16AdcRes[u8ActChannel] = ADCL;
+  gu16AdcRes[u8ActChannel] += (ADCH << 8);
+  if (u8ActChannel == ADC_CURRENT)
+  {
+    if (gu16AdcRes [ADC_CURRENT] > 512)
+    {
+      gu16AdcRes [ADC_CURRENT] = 0;
+    }
+    else
+    {
+      gu16AdcRes [ADC_CURRENT] = 512 - gu16AdcRes [ADC_CURRENT];
+    }
+    //AddToFloatAvg (&gFilterCurrent, gu16AdcRes [ADC_CURRENT]);
+  }
+
+  u8ActChannel += 1;
+  if (u8ActChannel == USED_ADC_CHANNELS)
+     u8ActChannel = 0;
+  if (gbStarted)
+  {
+    u8ActChannel = ADC_CURRENT; 
+  }
+  ADMUX = (ADMUX & 0xE0) | u8ActChannel;
+}
+
+//==========================================================
+int main (void)
+{ 
+  //++++++++++++++++++++++++++++++++++++
+  //declaration
+  char sBuffer [20];
+  char sState [7];
+  unsigned char bMotorLeft = 0, bTorqueReached = 0;
+  uint16_t u16Current, u16CurrentZero, u16CurrentReached, u16CurrentReachedRAW;
+  uint16_t u16SetpointSpeed = 0, u16LCDTrigger = 1;
+  double fTorque, fSetpointTorque = 0.0, fReachedTorque = 0.0, fReachedTorqueRAW = 0.0;
+
+  const double fCurrentStep = 0.04932; //From Sensortype
+  const double fTorqueConstant = 0.630; //Nm/A from Motortype
+  const double fTorqueSetpointStep = 0.01230469;  //From Sensortype 0.009885
+  const double fSetpointTorqueMax = 9.0; //From Sensortype / PowwerSupply Nm
+  double fCurrTorqueConst = fCurrentStep * fTorqueConstant;
+  //Portsetup 
+  lcd_init ();
+
+  //PORTD = (1 << PD0) | (1 << PD1); //Activate Pullups 644 changed fuction 
+  //
+  //
+  //Lock Low Side of H-Bride !!!!
+  DDRD = (1 << PD4) | (1 << PD5) | (1 << PD6) ;  // Set Port Read Write (4,5,6)
+
+  //Enable Interrupts
+  sei ();
+
+  //Set Timer 0
+  //tc0_init ();
+  
+  //ADC initialisieren
+  ADC_init ();
+  // initialisieren und mit aktuellem wert fuellen
+  //FixMe InitFloatAvg (&gFilterCurrent, gu16AdcRes [ADC_CURRENT]);
+
+  //LCD Light ON
+  LCD_PORTDATA |=  (1 << LCD_LED);
+
+  // Disclaimer
+  set_cursor (0, 1);
+  lcd_string (" MS-7DA v2.00");
+  set_cursor (0, 2);
+  lcd_string (" mir.systems ");
+  for (u16LCDTrigger = 20000; u16LCDTrigger == 0; u16LCDTrigger--) _delay_ms (1);
+       
+  //Calibrate
+  u16CurrentZero = GetOutputValue (&gFilterCurrent);
+
+  while (1)
+  {
+     
+    //Read Current Vallue fom IC 
+    //Floating Average
+    //u16Current = GetOutputValue (&gFilterCurrent);
+    u16Current = gu16AdcRes [ADC_CURRENT];
+
+
+    gcStopped = ((PIND & (1 << PD0)) == 0);
+    if (gcStopped)
+    {
+      bMotorLeft = PIND & (1 << PD1);
+      if (bMotorLeft)
+      {
+        strcpy (sState, "Li");
+      }
+      else
+      {
+        strcpy (sState, "Re");
+      }
+      //Motor stopped so recalibrate
+      //FixMe bad idea u16CurrentZero = u16Current;
+      //Get potis
+      //u16SetpointSpeed = gu16AdcRes [ADC_SETPOINT_SPEED];
+      u16SetpointSpeed = 1023;
+      fSetpointTorque = (double) gu16AdcRes [ADC_SETPOINT_TORQUE] * fTorqueSetpointStep;
+      //Minumum Torque
+      if (fSetpointTorque < 0.5) 
+        fSetpointTorque = 0.5;
+      if (fSetpointTorque > fSetpointTorqueMax)
+        fSetpointTorque = fSetpointTorqueMax;
+    }
+    else
+    {
+      if (gu16SpeedDelay > 0)
+      gu16SpeedDelay --;
+    }
+
+    if (u16Current < u16CurrentZero)
+      u16Current = 0;
+    else
+      u16Current -= u16CurrentZero;
+ 
+    fTorque = (double)(u16Current) * fCurrTorqueConst;
+
+    //Start Motor left
+    if (((PIND & (1 << PD0)) == 1) && bMotorLeft && !bTorqueReached)
+    {
+      //Start Fw
+      PORTD |=  (1 << PD4);
+      PORTD &= ~(1 << PD5);
+      if (!gbStarted)
+      {
+        gbStarted = 1;
+      }
+    }
+    //Start Motor right
+    else if (((PIND & (1 << PD0)) == 1) && !bMotorLeft && !bTorqueReached)
+    {
+      //Start Rw
+      PORTD |=  (1 << PD5);
+      PORTD &= ~(1 << PD6);
+      if (!gbStarted)
+      {
+        gbStarted = 1;
+      }
+    }
+    else if (gcStopped)
+    {
+      PORTD &= ~(1 << PD4);
+      PORTD &= ~(1 << PD5);
+      //smooth stop   
+      gu16SpeedDelay = 0x02ff;
+      bTorqueReached = 0;
+      gbStarted = 0;
+    }
+    
+    //FixMe
+    if (1 &&  ((((fTorque > fSetpointTorque) && !bMotorLeft) || (fTorque > fSetpointTorqueMax) )&& !gu16SpeedDelay && !bTorqueReached) )
+    {
+      //Torque Stop after start ramp
+      PORTD &= ~(1 << PD4);
+      PORTD &= ~(1 << PD5);
+      
+      bTorqueReached = 1;
+      u16CurrentReached = u16Current;
+      u16CurrentReachedRAW = gu16AdcRes [ADC_CURRENT];
+      fReachedTorque = fTorque;
+      fReachedTorqueRAW = (double)(u16CurrentReachedRAW) * fCurrTorqueConst;
+      strcpy (sState, "ST");
+    }
+
+    //LCD
+    u16LCDTrigger ++;
+    if (u16LCDTrigger == 500)
+    {
+      #ifdef ADC_DEBUG
+        set_cursor (0, 1);
+        sprintf (sBuffer, "M%d v%d I%d" ,gu16AdcRes [ADC_SETPOINT_TORQUE], gu16AdcRes [ADC_CURRENT], gu16AdcRes [ADC_CURRENT_ALT]); 
+        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, "%s I%dA M%04.2fN", sState, u16CurrentReachedRAW, fReachedTorqueRAW);
+          #else
+            sprintf (sBuffer, "M erreicht");
+          #endif 
+          lcd_string (sBuffer);
+         }
+      #endif
+
+      if (bTorqueReached)
+      {
+        set_cursor (0, 2);
+        sprintf (sBuffer, "%s M=%03.2fNm ", sState, fReachedTorque);
+        //sprintf (sBuffer, "%s I%dA M%04.2fN", sState, u16CurrentReached, fReachedTorque);
+        lcd_string (sBuffer);
+      }
+      if (gcStopped)
+      {
+        set_cursor (0, 2);
+        sprintf (sBuffer, "%s           ", sState);
+        lcd_string (sBuffer);
+      }
+      u16LCDTrigger = 0;
+    }
+  }
+  return 1;
+}
+
+
+
+
+
+/*==========================================*/
+void tc0_init (void)
+{
+  // Set timer 0
+  // normal operation
+  TCCR0A =  (0 << WGM01) | (0 << WGM00) | \
+           (0 << CS02) | (1 << CS01) | (0 << CS00);
+
+  //Enable overflow interrupt
+  TIMSK0 |= (1 << TOIE0);
+}
+
+
+
+/*==========================================*/
+void ADC_init (void)
+{
+  //Seclect Channel
+  //set Reference Voltage to external
+  //next Versions internal 5V
+  ADMUX = (0 << REFS1) | (0 << REFS0);
+  //Prescaler 50kHz < CLK / Prescaler < 200 kHz  otherwise errors in LSBs!!!
+  //at 20MHz := 100 > 128
+  //enaable ADC and interupt
+  //Enable freeruning mode,
+  ADCSRA = (1 << ADEN) | (1 << ADIE) | (1 << ADATE)| (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); 
+  ADCSRA |= (1 << ADSC);
+}
+

firmware/main.c

@@ -0,0 +1,312 @@
+/*********************************************
+Screwdriver - Controller
+Compiler            : avr
+Chip type           : ATMEGA644
+MiR 2014
+*********************************************/
+#include <avr/io.h>
+#include <util/delay.h>
+#include <avr/interrupt.h>
+#include <string.h>
+#include <stdio.h>
+#include "lcd-routines.h"
+#include "floating_avg.h"
+
+
+//Constants
+#define USED_ADC_CHANNELS 4
+#define ADC_CURRENT 0
+#define ADC_CURRENT_ALT 1
+#define ADC_SETPOINT_SPEED 2
+#define ADC_SETPOINT_TORQUE 3
+
+//Devel
+//#define TORQUE_DEBUG //To debug floating AVG
+//#define ADC_DEBUG //To debug ADC channels
+
+//Prototypes
+void  tc0_init (void);
+void  ADC_init (void);
+uint16_t  ADC_sc (uint8_t u8ActChannel);
+
+//global Vars for ISR Access
+volatile unsigned char gcStopped;
+volatile unsigned char gbStarted = 0;
+volatile uint16_t gu16SpeedDelay;
+volatile uint16_t gu16AdcRes[USED_ADC_CHANNELS];
+volatile tFloatAvgFilter gFilterCurrent;
+
+
+//ISR Timer 0 - for future usage
+ISR (TIMER0_OVF_vect)
+{
+
+}
+
+//==========================================================
+int main (void)
+{ 
+  //++++++++++++++++++++++++++++++++++++
+  //declaration
+  char sBuffer [20];
+  char sState [7];
+  unsigned char bMotorLeft = 0, bTorqueReached = 0;
+  uint16_t u16Current, u16CurrentZero, u16CurrentReached, u16CurrentReachedRAW;
+  uint16_t f1 = 0,f2 = 0;
+  uint16_t u16SetpointSpeed = 0, u16LCDTrigger = 1;
+  double fTorque, fSetpointTorque = 0.0, fReachedTorque = 0.0, fReachedTorqueRAW = 0.0;
+
+  const double fCurrentStep = 0.04932; //From Sensortype
+  const double fTorqueConstant = 0.630; //Nm/A from Motortype
+  const double fTorqueSetpointStep = 0.01230469;  //From Sensortype 0.009885
+  const double fSetpointTorqueMax = 9.0; //From Sensortype / PowwerSupply Nm
+  double fCurrTorqueConst = fCurrentStep * fTorqueConstant;
+  //Portsetup 
+  lcd_init ();
+
+  //PORTD = (1 << PD0) | (1 << PD1); //Activate Pullups 644 changed fuction 
+  //
+  //
+  //Lock Low Side of H-Bride !!!!
+  DDRD = (1 << PD4) | (1 << PD5) | (1 << PD6) ;  // Set Port Read Write (4,5,6)
+
+  //Enable Interrupts
+  sei ();
+
+  //Set Timer 0
+  //tc0_init ();
+  
+  //ADC initialisieren
+  ADC_init ();
+  // initialisieren und mit aktuellem wert fuellen
+  //FixMe InitFloatAvg (&gFilterCurrent, gu16AdcRes [ADC_CURRENT]);
+
+  //LCD Light ON
+  LCD_PORTDATA |=  (1 << LCD_LED);
+
+  // Disclaimer
+  set_cursor (0, 1);
+  lcd_string (" MS-7DA v2.00");
+  set_cursor (0, 2);
+  lcd_string (" mir.systems ");
+
+  gu16AdcRes [ADC_SETPOINT_SPEED] = ADC_sc (ADC_SETPOINT_SPEED);
+  gu16AdcRes [ADC_SETPOINT_TORQUE] = ADC_sc (ADC_SETPOINT_TORQUE);
+  u16Current =  ADC_sc (ADC_CURRENT);
+  for (u16LCDTrigger = 10000; u16LCDTrigger == 0; u16LCDTrigger--) _delay_ms (1);
+  u16LCDTrigger = 499;
+       
+  //Calibrate
+  u16CurrentZero = GetOutputValue (&gFilterCurrent);
+  u16CurrentZero = u16Current;
+
+  while (1)
+  {
+     
+    gcStopped = ((PIND & (1 << PD0)) == 0);
+
+    if (gcStopped)
+    {
+      gu16AdcRes [ADC_SETPOINT_SPEED] = ADC_sc (ADC_SETPOINT_SPEED);
+      gu16AdcRes [ADC_SETPOINT_TORQUE] = ADC_sc (ADC_SETPOINT_TORQUE);
+      #ifdef ADC_DEBUG
+        gu16AdcRes [ADC_CURRENT] = ADC_sc (ADC_CURRENT);
+      #endif
+    }
+    else
+    {
+      //Read Current Vallue fom IC 
+      //Floating Average
+      //u16Current = GetOutputValue (&gFilterCurrent);
+      gu16AdcRes [ADC_CURRENT] = ADC_sc (ADC_CURRENT);
+      u16Current =  ADC_sc (ADC_CURRENT);
+    }
+
+    if (gcStopped)
+    {
+      bMotorLeft = PIND & (1 << PD1);
+      if (bMotorLeft)
+      {
+        strcpy (sState, "Li");
+      }
+      else
+      {
+        strcpy (sState, "Re");
+      }
+      //Motor stopped so recalibrate
+      //FixMe bad idea u16CurrentZero = u16Current;
+      //Get potis
+      //u16SetpointSpeed = gu16AdcRes [ADC_SETPOINT_SPEED];
+      u16SetpointSpeed = 1023;
+      fSetpointTorque = (double) gu16AdcRes [ADC_SETPOINT_TORQUE] * fTorqueSetpointStep;
+      //Minumum Torque
+      if (fSetpointTorque < 0.5) 
+        fSetpointTorque = 0.5;
+      if (fSetpointTorque > fSetpointTorqueMax)
+        fSetpointTorque = fSetpointTorqueMax;
+    }
+    else
+    {
+      if (gu16SpeedDelay > 0)
+      gu16SpeedDelay --;
+    }
+
+    if (u16Current < u16CurrentZero)
+      u16Current = 0;
+    else
+      u16Current -= u16CurrentZero;
+ 
+    fTorque = (double)(u16Current) * fCurrTorqueConst;
+
+    //Start Motor left
+    if (((PIND & (1 << PD0)) == 1) && bMotorLeft && !bTorqueReached)
+    {
+      //Start Fw
+      PORTD |=  (1 << PD4);
+      PORTD &= ~(1 << PD5);
+      if (!gbStarted)
+      {
+        gbStarted = 1;
+      }
+    }
+    //Start Motor right
+    else if (((PIND & (1 << PD0)) == 1) && !bMotorLeft && !bTorqueReached)
+    {
+      //Start Rw
+      PORTD |=  (1 << PD5);
+      PORTD &= ~(1 << PD6);
+      if (!gbStarted)
+      {
+        gbStarted = 1;
+      }
+    }
+    else if (gcStopped)
+    {
+      PORTD &= ~(1 << PD4);
+      PORTD &= ~(1 << PD5);
+      //smooth stop   
+      gu16SpeedDelay = 0x02ff;
+      bTorqueReached = 0;
+      gbStarted = 0;
+    }
+    
+    if (1 &&  ((((fTorque > fSetpointTorque) && !bMotorLeft) || (fTorque > fSetpointTorqueMax) )&& !gu16SpeedDelay && !bTorqueReached) )
+    {
+      //Torque Stop after start ramp
+      PORTD &= ~(1 << PD4);
+      PORTD &= ~(1 << PD5);
+      
+      bTorqueReached = 1;
+      u16CurrentReached = u16Current;
+      u16CurrentReachedRAW = gu16AdcRes [ADC_CURRENT];
+      fReachedTorque = fTorque;
+      fReachedTorqueRAW = (double)(u16CurrentReachedRAW) * fCurrTorqueConst;
+      strcpy (sState, "ST");
+    }
+
+    //LCD
+    u16LCDTrigger ++;
+    if (u16LCDTrigger == 500)
+    {
+      #ifdef ADC_DEBUG
+        set_cursor (0, 1);
+        sprintf (sBuffer, "M%d I1%d I2%d" ,gu16AdcRes [ADC_SETPOINT_TORQUE], gu16AdcRes [ADC_CURRENT], gu16AdcRes [ADC_CURRENT_ALT]); 
+        lcd_string (sBuffer);
+      #else
+        if (gcStopped)
+        {
+          set_cursor (0, 1);
+          f1 = fSetpointTorque;
+          f2 = fSetpointTorque * 10;
+          f2 = f2 % 10;
+          sprintf (sBuffer, "v=%3d%% M=%03d.%1dNm", ((u16SetpointSpeed -  (u16SetpointSpeed /1000) * u16SetpointSpeed %1000)/ 10), f1, f2);
+          lcd_string (sBuffer);
+         }
+        if (bTorqueReached)
+        {
+          set_cursor (0, 1);
+          #ifdef TORQUE_DEBUG
+            sprintf (sBuffer, "%s I%dA M%04.2fN", sState, u16CurrentReachedRAW, fReachedTorqueRAW);
+          #else
+            sprintf (sBuffer, "M erreicht");
+          #endif 
+          lcd_string (sBuffer);
+         }
+      #endif
+
+      if (bTorqueReached)
+      {
+        set_cursor (0, 2);
+       f1 = fSetpointTorque;
+       f2 = fSetpointTorque * 100;
+       f2 = f2 % 100;
+        sprintf (sBuffer, "%s M=%03d.%02dNm ", sState, f1, f2);
+        //sprintf (sBuffer, "%s I%dA M%04.2fN", sState, u16CurrentReached, fReachedTorque);
+        lcd_string (sBuffer);
+      }
+      if (gcStopped)
+      {
+        set_cursor (0, 2);
+        sprintf (sBuffer, "%s           ", sState);
+        lcd_string (sBuffer);
+      }
+      u16LCDTrigger = 0;
+    }
+  }
+  return 1;
+}
+
+
+
+
+
+/*==========================================*/
+void tc0_init (void)
+{
+  // Set timer 0
+  // normal operation
+  TCCR0A =  (0 << WGM01) | (0 << WGM00) | \
+           (0 << CS02) | (1 << CS01) | (0 << CS00);
+
+  //Enable overflow interrupt
+  TIMSK0 |= (1 << TOIE0);
+}
+
+
+
+/*==========================================*/
+void ADC_init (void)
+{
+  //Seclect Channel
+  //set Reference Voltage to external
+  //next Versions internal 5V
+  ADMUX = (0 << REFS1) | (0 << REFS0);
+  //Prescaler 50kHz < CLK / Prescaler < 200 kHz  otherwise errors in LSBs!!!
+  //at 20MHz := 100 > 128
+  //enaable ADC and no interupt ADIE
+  //Disable freeruning mode ADATE,
+  ADCSRA = (0 << ADEN) | (0 << ADIE) | (0 << ADATE)| (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); 
+}
+
+//ADC single conversion
+uint16_t ADC_sc (uint8_t u8ActChannel)
+{
+  uint16_t val;
+  ADCSRA |= (1 << ADEN);
+  ADMUX = (ADMUX & 0xE0) | u8ActChannel;
+  ADCSRA |= (1 << ADSC);
+  while (ADCSRA & (1 << ADSC));
+  val = ADCW;
+  ADCSRA &= ~(1 << ADEN);
+  if (u8ActChannel == ADC_CURRENT)
+  {
+    if (val > 512)
+      val = val -512;
+    else
+      val = 0;
+    //AddToFloatAvg (&gFilterCurrent, gu16AdcRes [ADC_CURRENT]);
+  }
+  return val;
+}
+
+

screwdriver.xcodeproj/project.pbxproj

@@ -0,0 +1,102 @@
+// !$*UTF8*$!
+{
+	archiveVersion = 1;
+	classes = {
+	};
+	objectVersion = 42;
+	objects = {
+
+/* Begin PBXFileReference section */
+		4079525E0CEA38CC00E202DC /* main.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = main.c; sourceTree = "<group>"; };
+		4079525F0CEA38CC00E202DC /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; path = Makefile; sourceTree = "<group>"; };
+/* End PBXFileReference section */
+
+/* Begin PBXGroup section */
+		4079523B0CEA382200E202DC = {
+			isa = PBXGroup;
+			children = (
+				4079525D0CEA38CC00E202DC /* firmware */,
+			);
+			sourceTree = "<group>";
+		};
+		4079525D0CEA38CC00E202DC /* firmware */ = {
+			isa = PBXGroup;
+			children = (
+				4079525F0CEA38CC00E202DC /* Makefile */,
+				4079525E0CEA38CC00E202DC /* main.c */,
+			);
+			path = firmware;
+			sourceTree = "<group>";
+		};
+/* End PBXGroup section */
+
+/* Begin PBXLegacyTarget section */
+		407952600CEA391500E202DC /* firmware */ = {
+			isa = PBXLegacyTarget;
+			buildArgumentsString = $ACTION;
+			buildConfigurationList = 407952610CEA393300E202DC /* Build configuration list for PBXLegacyTarget "firmware" */;
+			buildPhases = (
+			);
+			buildToolPath = /usr/bin/make;
+			buildWorkingDirectory = firmware;
+			dependencies = (
+			);
+			name = firmware;
+			passBuildSettingsInEnvironment = 1;
+			productName = "Build All";
+		};
+/* End PBXLegacyTarget section */
+
+/* Begin PBXProject section */
+		4079523D0CEA382200E202DC /* Project object */ = {
+			isa = PBXProject;
+			buildConfigurationList = 4079523E0CEA382200E202DC /* Build configuration list for PBXProject "TemplateProject" */;
+			compatibilityVersion = "Xcode 2.4";
+			hasScannedForEncodings = 0;
+			mainGroup = 4079523B0CEA382200E202DC;
+			projectDirPath = "";
+			projectRoot = "";
+			targets = (
+				407952600CEA391500E202DC /* firmware */,
+			);
+		};
+/* End PBXProject section */
+
+/* Begin XCBuildConfiguration section */
+		407952400CEA382200E202DC /* Release */ = {
+			isa = XCBuildConfiguration;
+			buildSettings = {
+				COPY_PHASE_STRIP = YES;
+			};
+			name = Release;
+		};
+		407952630CEA393300E202DC /* Release */ = {
+			isa = XCBuildConfiguration;
+			buildSettings = {
+				PATH = "$(PATH):/usr/local/CrossPack-AVR/bin";
+			};
+			name = Release;
+		};
+/* End XCBuildConfiguration section */
+
+/* Begin XCConfigurationList section */
+		4079523E0CEA382200E202DC /* Build configuration list for PBXProject "TemplateProject" */ = {
+			isa = XCConfigurationList;
+			buildConfigurations = (
+				407952400CEA382200E202DC /* Release */,
+			);
+			defaultConfigurationIsVisible = 0;
+			defaultConfigurationName = Release;
+		};
+		407952610CEA393300E202DC /* Build configuration list for PBXLegacyTarget "firmware" */ = {
+			isa = XCConfigurationList;
+			buildConfigurations = (
+				407952630CEA393300E202DC /* Release */,
+			);
+			defaultConfigurationIsVisible = 0;
+			defaultConfigurationName = Release;
+		};
+/* End XCConfigurationList section */
+	};
+	rootObject = 4079523D0CEA382200E202DC /* Project object */;
+}

screwdriver.xcodeproj/project.xcworkspace/contents.xcworkspacedata

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Workspace
+   version = "1.0">
+   <FileRef
+      location = "self:screwdriver.xcodeproj">
+   </FileRef>
+</Workspace>

screwdriver.xcodeproj/xcuserdata/michi.xcuserdatad/xcschemes/firmware.xcscheme

@@ -0,0 +1,77 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Scheme
+   LastUpgradeVersion = "0610"
+   version = "1.3">
+   <BuildAction
+      parallelizeBuildables = "YES"
+      buildImplicitDependencies = "YES">
+      <BuildActionEntries>
+         <BuildActionEntry
+            buildForTesting = "YES"
+            buildForRunning = "YES"
+            buildForProfiling = "YES"
+            buildForArchiving = "YES"
+            buildForAnalyzing = "YES">
+            <BuildableReference
+               BuildableIdentifier = "primary"
+               BlueprintIdentifier = "407952600CEA391500E202DC"
+               BuildableName = "firmware"
+               BlueprintName = "firmware"
+               ReferencedContainer = "container:screwdriver.xcodeproj">
+            </BuildableReference>
+         </BuildActionEntry>
+      </BuildActionEntries>
+   </BuildAction>
+   <TestAction
+      selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+      selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+      shouldUseLaunchSchemeArgsEnv = "YES"
+      buildConfiguration = "Release">
+      <Testables>
+      </Testables>
+   </TestAction>
+   <LaunchAction
+      selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+      selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+      launchStyle = "0"
+      useCustomWorkingDirectory = "NO"
+      buildConfiguration = "Release"
+      ignoresPersistentStateOnLaunch = "NO"
+      debugDocumentVersioning = "YES"
+      allowLocationSimulation = "YES">
+      <MacroExpansion>
+         <BuildableReference
+            BuildableIdentifier = "primary"
+            BlueprintIdentifier = "407952600CEA391500E202DC"
+            BuildableName = "firmware"
+            BlueprintName = "firmware"
+            ReferencedContainer = "container:screwdriver.xcodeproj">
+         </BuildableReference>
+      </MacroExpansion>
+      <AdditionalOptions>
+      </AdditionalOptions>
+   </LaunchAction>
+   <ProfileAction
+      shouldUseLaunchSchemeArgsEnv = "YES"
+      savedToolIdentifier = ""
+      useCustomWorkingDirectory = "NO"
+      buildConfiguration = "Release"
+      debugDocumentVersioning = "YES">
+      <MacroExpansion>
+         <BuildableReference
+            BuildableIdentifier = "primary"
+            BlueprintIdentifier = "407952600CEA391500E202DC"
+            BuildableName = "firmware"
+            BlueprintName = "firmware"
+            ReferencedContainer = "container:screwdriver.xcodeproj">
+         </BuildableReference>
+      </MacroExpansion>
+   </ProfileAction>
+   <AnalyzeAction
+      buildConfiguration = "Release">
+   </AnalyzeAction>
+   <ArchiveAction
+      buildConfiguration = "Release"
+      revealArchiveInOrganizer = "YES">
+   </ArchiveAction>
+</Scheme>

screwdriver.xcodeproj/xcuserdata/michi.xcuserdatad/xcschemes/xcschememanagement.plist

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>SchemeUserState</key>
+	<dict>
+		<key>firmware.xcscheme</key>
+		<dict>
+			<key>orderHint</key>
+			<integer>0</integer>
+		</dict>
+	</dict>
+	<key>SuppressBuildableAutocreation</key>
+	<dict>
+		<key>407952600CEA391500E202DC</key>
+		<dict>
+			<key>primary</key>
+			<true/>
+		</dict>
+	</dict>
+</dict>
+</plist>