When the following function is called, the Nano stops responding or doing anything.
Code: Select all
void thumbMove() { //interupt the program and move the thumb
unsigned long interrupt_time = millis();
if (thumbPos == 0 && interrupt_time - last_interrupt_time > 100) { // then finger is unlocked so lock it
Serial.println("Closing Thumb");
pwm.setPWM(1, 0, 515);
thumbPos = 1;
} else {
// then thumb is closed so open it
if (interrupt_time - last_interrupt_time > 100) {
Serial.println("Opening Thumb");
pwm.setPWM(1, 0, 300);
thumbPos = 0;
}
}
last_interrupt_time = interrupt_time;
}It doesn't appear to be related to power draw as the same problem occurs with the servos disconnected.
Here's the full code. I know some steps could be compacted, but haven't got to the optimisation stage yet!
Code: Select all
// setup PWN module
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)
// set servo number on PWN module
uint8_t servonum = 0;
// prep variables
int pos;
int pos2 = 0;
int lastpos = 0;
int posDifference = 0;
volatile int thumbPos = 0;
volatile int fingerLock = 0;
static unsigned long last_interrupt_time = 0;
//setup buttons
int thumbPin = 2; //3 switch-wrist
int maxPin = 5; // 3 switch-middle
int minPin = 6; //3 switch-finger
int fingerLockPin = 3; //single switch
volatile int thumbVal = 0; // variable for reading the pin status
// volatile int maxVal = 10;
// volatile int minVal = 500;
volatile int lockVal = 0;
void setup() {
// setup serial monitor
Serial.begin(9600);
//setup PWM
pwm.begin();
pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
yield();
//setup buttons
pinMode(thumbPin, INPUT); // declare pushbutton as input
digitalWrite(thumbPin, HIGH); // set pin to high
pinMode(minPin, INPUT); // declare pushbutton as input
digitalWrite(minPin, HIGH); // set pin to high
pinMode(maxPin, INPUT); // declare pushbutton as input
digitalWrite(maxPin, HIGH); // set pin to high
pinMode(fingerLockPin, INPUT); // declare pushbutton as input
digitalWrite(fingerLockPin, HIGH); // set pin to high
// setup button interupts
attachInterrupt(digitalPinToInterrupt(thumbPin), thumbMove, LOW);
attachInterrupt(digitalPinToInterrupt(minPin), resetMin, LOW);
attachInterrupt(digitalPinToInterrupt(maxPin), resetMax, LOW);
attachInterrupt(digitalPinToInterrupt(fingerLockPin), lockFinger, LOW);
}
void loop() {
//servo scaling and clipping
pos = getSensorAverageValue(0, 10, 5);
Serial.println(pos);
if (pos > 500) pos = 500;
//decide if the change is big enough to justify a move
posDifference = pos - lastpos;
if (posDifference >=5 || posDifference <=-5) {
//print state
// Serial.println(pos);
// work out change in state
lastpos = pos;
// Serial.println(pos);
pos2 = map(pos, 25, 150, 0, 180);
pos2 = constrain(pos2, 0, 180);
// map - variable, original min, original max, new min, new max
uint16_t pulselen = map(pos2, 1, 150, SERVOMIN, SERVOMAX);
if (fingerLock == 0){
// finger is unlocked so set position
// set servo position
pwm.setPWM(servonum, 0, pulselen);
} else {
//finger is locked so reset it?
pwm.setPWM(servonum, 0, 0);
}
}
}
// get an average value from a jumpy or erratic input sensor.
const int getSensorAverageValue (const int sensorPin,
const int numberOfSamples,
const long timeGap)
{
static int currentSample; // current sensor sample.
static int currentValue; // current sensor value.
// current value works as a sum counter.
currentValue = 0;
// get sensor samples with delay and calculate the sum.
for (int i = 0; i < numberOfSamples; i++) {
// get sensor sample.
currentSample = analogRead(sensorPin);
// add sample to the sum counter.
currentValue += currentSample;
// delay some time for the next sample.
delay(timeGap);
}
// get the average sensor value (ignore the fraction).
return (currentValue / numberOfSamples);
}
void thumbMove() { //interupt the program and move the thumb
unsigned long interrupt_time = millis();
if (thumbPos == 0 && interrupt_time - last_interrupt_time > 100) { // then finger is unlocked so lock it
Serial.println("Closing Thumb");
//pwm.setPWM(1, 0, 515);
thumbPos = 1;
} else {
// then thumb is closed so open it
if (interrupt_time - last_interrupt_time > 100) {
Serial.println("Opening Thumb");
//pwm.setPWM(1, 0, 300);
thumbPos = 0;
}
}
last_interrupt_time = interrupt_time;
}
void resetMin() { //interupt the program and set the Min
Serial.println("Minimum set");
// to be added
}
void resetMax() { //interupt the program and set the Max
Serial.println("Max set - updated");
// to be added
}