Megaservos

#include <SPI.h>
#include <LiquidCrystal.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#include <ctype.h>
#include <math.h>
#include <avr/io.h>


#define HW_4017_RESET_PIN         47
#define HW_4017_CLOCK_PIN         13

#define _4017_RESET_PORT        PORTB
#define _4017_CLOCK_PORT        PORTB
#define _4017_RESET_DDR         DDRB
#define _4017_CLOCK_DDR         DDRB
#define _4017_RESET_PIN         PINB0
#define _4017_CLOCK_PIN         PINB1

#define SERVO_OCR		OCR1A
#define SERVO_ENABLE            OCIE1A
#define SERVO_FLAG		OCF1A
#define SERVO_FORCE		FOC1A
#define SERVO_COM0		COM1A0
#define SERVO_COM1		COM1A1

#if defined (__AVR_ATmega8__)
#define TIMER_SK	TIMSK
#define TIMER_IFR	TIFR
#define FORCE_REG	TCCR1A
#else
#define TIMER_SK	TIMSK1
#define TIMER_IFR	TIFR1
#define FORCE_REG	TCCR1C
#endif

#ifndef _BV
#define _BV(bit) (1 << (bit))
#endif
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

#define CLOCK 16
#define SYS_TICS_OF_USEC(us) (uint16_t)((us)*CLOCK)

#define Chop(_x, _min, _max) ( (_x) < (_min) ? (_min) : (_x) > (_max) ? (_max) : (_x) )
#define SetServo(x, v) servo_widths[x] = SYS_TICS_OF_USEC(Chop(v,700,2400));

#define _4017_NB_CHANNELS 10

#define ON  1
#define OFF 0
#define OneSecond  1000

#define ledPin	13

unsigned int servo_widths[_4017_NB_CHANNELS];

void Servo_4017_Set_Pulse_Width( int ServoNum, int pulseWidth )
{
	servo_widths[ServoNum] = SYS_TICS_OF_USEC(pulseWidth);
}
void Servo_4017_init( void ) {
	uint8_t i;

	/* Configure 2 Arduino pins as outputs for controlling the 4017 reset and clock lines  */
	digitalWrite(HW_4017_RESET_PIN, 0);
	digitalWrite(HW_4017_CLOCK_PIN, 0);
	pinMode(HW_4017_RESET_PIN, OUTPUT);
	pinMode(HW_4017_CLOCK_PIN, OUTPUT);

	digitalWrite(HW_4017_RESET_PIN, 1);    // Reset the decade counter 
	digitalWrite(HW_4017_CLOCK_PIN, 0);    // Lower the clock line

	/* Set all servos at their midpoints              */
	for( i=0 ; i < _4017_NB_CHANNELS ; i++ )
		servo_widths[i] = SYS_TICS_OF_USEC(1500); // SetServo(i,1500);        

	// ----------------------------------------------------------------------------------------- 
	// Configure Timer 1 and setup the Output Compare registers...

	/* Timer1 @ Clk/1: System clock */
	TCCR1A = 0x00;
	TCCR1B = _BV(CS10);

	SERVO_OCR = 0x7FFF;                  /* Set servos to go off some long time from now   */
	TCCR1A |=  _BV(SERVO_COM0 );         /* Set output compare to toggle the output bits   */

#ifdef SERVOS_FALLING_EDGE
	/* Starts CLOCK high for the falling edge case    */
	FORCE_REG |= _BV(SERVO_FORCE);
	// Serial.println("Falling Edge Active!");
#endif

	/* Clear the interrupt flags in case they are set */
	TIMER_IFR = _BV(SERVO_FLAG);

	digitalWrite(HW_4017_RESET_PIN, 0);    // Lower the decade counter reset line to start it running 

	/* Enable our output compare interrupts */
	TIMER_SK |= _BV(SERVO_ENABLE);
	Serial.println("finish1");
}
ISR( TIMER1_COMPA_vect )
{
	static uint8_t servo = 0;
	uint16_t width;

#ifdef SERVOS_FALLING_EDGE
#define RESET_WIDTH SYS_TICS_OF_USEC(1000)
#define FIRST_PULSE_WIDTH SYS_TICS_OF_USEC(100)
	if (servo == _4017_NB_CHANNELS) {
		sbi( _4017_RESET_PORT, _4017_RESET_PIN );    // set Reset pin high

		/** Start a long 1ms reset, keep clock low */
		SERVO_OCR += RESET_WIDTH;
		servo++;
		return;
	}

	if (servo > _4017_NB_CHANNELS) {
		/** Clear the reset, the clock has been toggled high */
		cbi( _4017_RESET_PORT, _4017_RESET_PIN );    // clear reset pin

		/** Start a short pulse-like period */
		SERVO_OCR += FIRST_PULSE_WIDTH;
		servo = 0;   /* Starts a new sequence of pulses next time */
		return;
	}

#else
	if (servo >= _4017_NB_CHANNELS) {
		sbi( _4017_RESET_PORT, _4017_RESET_PIN );     // set reset pin High
		servo = 0;
		// FIXME: 500 ns required by 4017 reset ???? why does it work without!
		//asm( "nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop;" );

		cbi( _4017_RESET_PORT, _4017_RESET_PIN );   // clear reset pin
	}
#endif

	width = servo_widths[servo];
	SERVO_OCR += width;
	FORCE_REG |= _BV(SERVO_FORCE);
	servo++;

}

void Servo_4017_init( void );
void Servo_4017_Set_Pulse_Width( int ServoNum, int pulseWidth );

// --------------------------------------------------------------------------------------------------------------------------------------------
/* Global Variable definitions */

uint16_t sWidth[8], sInc[8];

unsigned long myTimer = 0;     // general purpose timer 
unsigned long OneSecTimer = 0;

int LED_On = 0;

LiquidCrystal lcd(12);
char keypressed = 0;
int keyboardPin = 0; // Analog input pin that the keypad is attached to
int keyboardValue = 0;  // value read from the keyboard

int numCount = 0;
char pincode[4];
int state;

void setup(){
	Serial.begin(9600); //hardware serial to PC
	// setup LCD number of columns ans rows
	lcd.begin(16, 2);
	// print message on LCd
	lcd.print("Enter the pin:");
	lcd.setCursor(0,1);    

	// servos
	int n;
	Serial.begin(9600);     // debug output over serial line
	pinMode(ledPin, OUTPUT);
	digitalWrite(ledPin, ON);
	LED_On = 1;
	delay(OneSecond);

	Serial.println("---");
	Serial.println("Arduino 4017 Servo Driver v1.0");
	delay(OneSecond);
	digitalWrite(ledPin, OFF);

	for (n=0; n<8; n++)   // Set all servos to their center positions.
	{
		sWidth[n] = 1500;
		sInc[n] = 3;  // setup each servo to move at a different speed
	}
	sInc[0] = 2;
	Servo_4017_init();	/* Init Timer1 and setup Output Compare interrupt */

	digitalWrite(ledPin, ON);
	OneSecTimer = millis();
	myTimer = millis();
}

void precision_loop ()
{
	int n;
	if ((millis() - myTimer) >= 20)  // 100ms is arbitrary interval for demo
	{
		myTimer = millis();

		for (n=0; n<8; n++)   // Keep servo 0 at center position - move all the rest.  Demos NO jitter on Servo 0. 
		{
			sWidth[n] += sInc[n];
			Servo_4017_Set_Pulse_Width(n, sWidth[n]);
			if ((sWidth[n] > 2100) || (sWidth[n] < 900))  // reverse direction at end of travel
				sInc[n] = -sInc[n];
		}
	}

	if ((millis() - OneSecTimer) >= OneSecond)	  // this goes off once per second
	{
		OneSecTimer = millis();
		LED_On = !LED_On;
		digitalWrite(ledPin, LED_On);					
	}
}

void loop(){
	// keypad start
	keyboardValue = analogRead(keyboardPin); 
	while (keyboardValue < 25){
		keyboardValue = analogRead(keyboardPin);
	}
	readkeyboard(); 
	// keypad end
	precision_loop();
}

void LcdClearLine(int r)
{
	lcd.setCursor(0, r);
	for (int ii = 0; ii < 16; ii = ii + 1) {
		lcd.print(" ");
	}
}

//read the keyboard routine
void readkeyboard(){
	keyboardValue = analogRead(keyboardPin); // read the value (0-1023)
	if (keyboardValue <25){keypressed = 0;}
	if ((keyboardValue >25) && (keyboardValue < 67)){keypressed = '1';}
	if ((keyboardValue >67) && (keyboardValue < 108)){keypressed = '2';}
	if ((keyboardValue >108) && (keyboardValue < 162)){keypressed = '3';}
	if ((keyboardValue >162) && (keyboardValue < 253)){keypressed = '4';}
	if ((keyboardValue >253) && (keyboardValue < 361)){keypressed = '5';}
	if ((keyboardValue >361) && (keyboardValue < 479)){keypressed = '6';}
	if ((keyboardValue >479) && (keyboardValue < 619)){keypressed = '7';}
	if ((keyboardValue >619) && (keyboardValue < 765)){keypressed = '8';}
	if ((keyboardValue >765) && (keyboardValue < 819)){keypressed = '9';}
	if ((keyboardValue >819) && (keyboardValue < 889)){keypressed = '*';}
	if ((keyboardValue >889) && (keyboardValue < 938)){keypressed = '0';}
	if (keyboardValue >938){keypressed = '#';}
	//NOTE: the values used above are all halfway between the value obtained with each keypress in previous test sketch
	while (keyboardValue > 25) {
		//delay (100);
		keyboardValue = analogRead(keyboardPin); // read the value (0-1023)
	}//wait until key no longer being pressed before continuing
	switch (keypressed) {
	case '#':
		Serial.println("ENTER");
		LcdClearLine(0);
		lcd.setCursor(0, 0);
		lcd.print("Welcome");
		lcd.setCursor(0, 1);
		LcdClearLine(1);          
		// send pin to server to validate
		break;
	case '*':
		Serial.println("RESET");
		// clean entered pincode
		numCount = 0;
		//pincode = char[4];
		LcdClearLine(0);
		lcd.setCursor(0, 0);
		lcd.print("Enter the pin:");
		LcdClearLine(1);
		lcd.setCursor(0, 1);
		break;
	default:
		//if(numCount != 4){
		pincode[numCount] = (char) keypressed;
		Serial.println("NUM");
		lcd.write(keypressed);
		//OpenServo(keypressed- '0');
		numCount++;
		//}
	}

	Serial.println(keypressed);  // print the value back to the Serial view window on your PC
	// delay(1000);  // wait 1000 milliseconds before the next loop
}
//end of read the keyboard routine