Beginners: please read this post and this post before posting to the forum.
0 Members and 1 Guest are viewing this topic.
// Standard AVR Headers#include <avr/io.h>#include <stdio.h>#include <math.h>#include <string.h>#include <inttypes.h>#include <avr/interrupt.h>// Macros#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) // clear bit#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) // set bit// General Compiler Instruction#define F_CPU 16000000L#define __AVR_ATmega168__// end MCU and compiler configurations// begin servo settings and functions// pin definitions// drive on timer 0#define drivePort PORTD#define driveDDR DDRD#define leftDrivePin 4 // compare match A#define rightDrivePin 3 // compare match B// head on timer 0#define headPort PORTD#define headDDR DDRD#define xHeadPin 5 // compare match B#define yHeadPin 6 // compare match A// tweaks for 16MHz// trial and error needed here#define driveCenter 100#define driveLeftRange 30#define driveRightRange 30#define leftOffset -1#define rightOffset -1#define headCenter 100#define headXRange 30#define headYRange 30#define xOffset -1#define yOffset -1#define xTimerUpperLimit 130#define yTimerUpperLimit 130#define xTimerLowerLimit 60#define yTimerLowerLimit 60// variablesvolatile uint8_t servoPulseFlag0 = 0;volatile uint8_t leftDriveServoTime;volatile uint8_t rightDriveServoTime;volatile uint8_t xHeadServoTime;volatile uint8_t yHeadServoTime;ISR(SIG_OVERFLOW0){ // alternate between drive and head servos servoPulseFlag0++; if(bit_is_clear(servoPulseFlag0, 0)) { // drive servo OCR0A = leftDriveServoTime; OCR0B = rightDriveServoTime; // it's better to leave pin off if servo needs to be stopped if(OCR0A != (driveCenter + leftOffset)) { sbi(drivePort, leftDrivePin); } if(OCR0B != (driveCenter + rightOffset)) { sbi(drivePort, rightDrivePin); } } else { // head servo sbi(headPort, xHeadPin); sbi(headPort, yHeadPin); OCR0B = xHeadServoTime; OCR0A = yHeadServoTime; }}// after the right time has passed, end pulseISR(SIG_OUTPUT_COMPARE0A){ cbi(drivePort, leftDrivePin); cbi(headPort, yHeadPin);}ISR(SIG_OUTPUT_COMPARE0B){ cbi(drivePort, rightDrivePin); cbi(headPort, xHeadPin);}void servoInit(){ // clear ports cbi(drivePort, leftDrivePin); cbi(drivePort, rightDrivePin); cbi(headPort, xHeadPin); cbi(headPort, yHeadPin); // set as outputs sbi(driveDDR, leftDrivePin); sbi(driveDDR, rightDrivePin); sbi(headDDR, xHeadPin); sbi(headDDR, yHeadPin); // start the timer TCCR0B = 0b00000100; // timer start, divide by 256 TIMSK0 = 0b00000111; // interrupts on}void servoDrive(signed int leftSpeed, signed int rightSpeed){ // limit, or suffer errors if(leftSpeed < -127) { leftSpeed = 127; } else if(leftSpeed > 127) { leftSpeed = 127; } if(rightSpeed < -127) { rightSpeed = 127; } else if(rightSpeed > 127) { rightSpeed = 127; } signed int leftTimer = leftOffset + driveCenter + ((driveRightRange * leftSpeed) / 128); signed int rightTimer = rightOffset + driveCenter - ((driveRightRange * rightSpeed) / 128); leftDriveServoTime = leftTimer; rightDriveServoTime = rightTimer;}void servoDriveMixed(signed int xAxis, signed int yAxis){ signed int leftSpeed = 2 * ((yAxis / 2) + (xAxis / 2)); signed int rightSpeed = 2 * ((yAxis / 2) - (xAxis / 2)); servoDrive(leftSpeed, rightSpeed);}void servoHead(int xAxis, int yAxis){ signed int xTimer = xOffset + headCenter + ((headXRange * xAxis) / 127); signed int yTimer = yOffset + headCenter + ((headYRange * yAxis) / 127); // limit, or suffer errors if(xTimer > xTimerUpperLimit) { xTimer = xTimerUpperLimit; } else if(xTimer < xTimerLowerLimit) { xTimer = xTimerLowerLimit; } if(yTimer > yTimerUpperLimit) { yTimer = yTimerUpperLimit; } else if(yTimer < yTimerLowerLimit) { yTimer = yTimerLowerLimit; } xHeadServoTime = xTimer; yHeadServoTime = yTimer;}void servoDisable(){ TCCR0B = 0; // stop timer // turn off the pins cbi(drivePort, leftDrivePin); cbi(drivePort, rightDrivePin); cbi(headPort, xHeadPin); cbi(headPort, yHeadPin);}// end servo functionsvoid mainInit(){ sei(); servoInit(); servoDrive(0, 0); servoHead(0, 0);}void mainLoop(){ int i; int j; for(j = -127; j <= 127; j++) { for(i = 0; i < 25; i++) // change 25 to change delay between interval { servoDrive(j, j); servoHead(j, j); } } for(j = 127; j >= -127; j--) { for(i = 0; i < 25; i++) { servoDrive(j, j); servoHead(j, j); } }}int main(){ mainInit(); while(1) { mainLoop(); } return 0;}