Hi,
I have a custom lib for a servo, but digitalWrite it's not working. All I did was moving the code inside a lib, it worked perfectly before.
Here is the header:
/*
Servo_mod.h - Library for controlling one servo
Created by J. Antonio Blazquez, 8 December 2012
Release into public domain
*/
#ifndef Motor_h
#define Motor_h
#include <Arduino.h>
#include <avr/interrupt.h>
#include <inttypes.h>
class Motor{
public:
#define MAX_SERVO 151 //mínimo valor que puede tomar el servo sin romperse = 500*(tiempo entre interrupciones) - en microsegundos
#define MIN_SERVO 36 //máximo valor que puede tomar el servo sin romperse = 2500*(tiempo entre interrupciones)
#define VEL_SERVO 30
#define OPT_SPEED 0
#define OPT_ACC 1
Motor(int pin);
void actualiza_servo(); //Makes servo rotation smooth
void print_servo_options(); //Serial print of the options
void opciones(int opt); //Calls the opt function selected
void girar(int angle ); //Rotates the servo an angle between 0-180
void velocidad_giro(int velocidad); //Changes rotating velocity (values 0-10)
void aceleracion_giro(int aceleracion); //Changes acceleration(0:linear 1:cuadratic)
private:
int _vel_servo;
int _x; //Dato introducido por consola
int _oldValue,_newValue; //Variables auxiliares
int _option;
int _acelerador;
bool _girando;
};
#endif
Here the code:
/*
Motor.h - Library for controlling one servo
Created by J. Antonio Blazquez, 8 December 2012
Release into public domain
void actualiza_servo(); //Makes servo rotation smooth
void print_servo_options(); //Serial print of the options
void opciones(int opt); //Switch between the servo options
void girar(int angle ); //Rotates the servo an angle between 0-180
void velocidad_giro(int velocidad); //Changes rotating velocity (values 0-10)
void aceleracion_giro(int aceleracion); //Changes acceleration(0:linear 1:cuadratic)
*/
#include <Arduino.h>
#include <avr/interrupt.h>
#include "Motor.h"
static volatile int _servoPin;
static volatile int _servoPulse; //Min 500 us - 1500us - 2500us Max
static volatile int _counter;
Motor::Motor(int servPin){
_counter = 0;
_vel_servo = 30;
_servoPulse = 90; //Min 500 us - 1500us - 2500us Max
_x = 0; //Dato introducido por consola
_oldValue,_newValue; //Variables auxiliares
_option = 1;
_acelerador = 1;
_servoPin;
_girando = false;
_servoPin = servPin;
pinMode(_servoPin, OUTPUT); //Pin de datos al servo
_newValue = _servoPulse;
_oldValue = _servoPulse;
noInterrupts(); // disable all interrupts
/***************** CONFIGURACIÓN DEL TIMER ************************/
TCCR1A = 0X02; //Preescalado /8 -> 16MHz/8 = 2MHz = 0'0005 ms/ciclo
TCCR1B = 0X1A; //timer1 (16bits = 65536 ciclos) _counter MODO 14 FAST PWM
TIMSK1= 0x01; //interruption por desbordamiento
TCNT1= 0xFFE1; //Valor inicial de timer 65505 -> 30ciclos/interrupción
interrupts(); // enable all interrupts
}
ISR(TIMER1_OVF_vect) { //Se produce una interrupción cada 0'015 ms
TCNT1= 0xFFE1; //Valor inicial de timer 65505 -> 30ciclos/interrupción
// ->
_counter++; //Suma 15 us cada interrupción
if (_counter < _servoPulse){
digitalWrite(_servoPin, HIGH);
}else{
digitalWrite(_servoPin, LOW);
}
if (_counter > 1333){
_counter=0;
}
}
void Motor::actualiza_servo(){
if(_option==OPT_ACC){
if (_newValue<_oldValue){
_servoPulse-=_acelerador;
_oldValue-=_acelerador;
Serial.println(_acelerador);
delay(_vel_servo);
}else if(_newValue>_oldValue){
_servoPulse+=_acelerador;
_oldValue+=_acelerador;
Serial.println(_acelerador);
delay(_vel_servo);
}
}
if(_option==OPT_SPEED){
if (_newValue<_oldValue){
_servoPulse-=1;
_oldValue-=1;
delay(_vel_servo);
}else if(_newValue>_oldValue){
_servoPulse+=1;
_oldValue+=1;
delay(_vel_servo);
}
if (_newValue == _servoPulse){
_girando = false;
}
}
_acelerador = _acelerador << 1;
if(_acelerador > abs(_newValue-_oldValue)){
_acelerador = 1;
}
}
void Motor :: print_servo_options(){
Serial.println("Opciones:");
Serial.println("1.- Girar 2.- Cambiar Velocidad 3.- Aceleracion geometrica 4.- Aceleracion lineal");
}
void Motor :: opciones(int opt){
int data;
switch (opt){
case 1:
Serial.println("Introducir Angulo (0 -180) :");
while(Serial.available()==0);
data = Serial.parseInt();
girar(data);
break;
case 2:
Serial.println("Introducir Velocidad (0-10): ");
while(Serial.available()==0);
data = Serial.parseInt();
velocidad_giro(data);
break;
case 3:
_option = 1;
Serial.println("Aceleracion cuadratica");
break;
case 4:
_option = 0;
Serial.println("Aceleracion lineal");
break;
}
}
void Motor :: girar(int angle ){
if (angle>=0 && angle<=180){
_oldValue = _servoPulse;
_newValue = map(angle,0,180,MIN_SERVO,MAX_SERVO);
_counter = 0;
_girando = true;
Serial.print("Angulo de giro: ");
Serial.println(angle);
while (_girando)
actualiza_servo();
}else{
Serial.println("Wrong value: Introduce un valor de 0 a 180 ");
}
}
void Motor :: velocidad_giro(int velocidad){
if(velocidad >=0 && velocidad <= 10){
_vel_servo = map(velocidad,10,0,0,300);
Serial.print("Nueva velocidad: ");
Serial.println(_vel_servo);
}else{
Serial.println("Wrong value: Introduce un valor de 0 a 10 ");
}
}
// aceleracion = 0 -> lineal
// aceleracion = 1 -> v^2
void Motor :: aceleracion_giro(int aceleracion){
_option = aceleracion;
}
And here an example of use:
#include <Motor.h>
int pin = 10;
Motor servo(pin);
int opt;
void setup(){
Serial.begin(9600);
servo.print_servo_options();
}
void loop(){
if (Serial.available()>0){
opt = Serial.parseInt();
servo.opciones(opt);
}
}
I have tested and the ISR runs and the variable _counter changes over time, but I don't know why the pin value doesn't change.