TomGeorge:
Hi,
Okay, can you now post your code?
When you were writing your code did you do it in stages?
If so at what stage did this problem occur?
Tom... 
#include <DueTimer.h>
#include <PID_v1.h>
#include "EncoderReader.h"
// encoder
#define clkPin 52
#define csPin 53
// link 1
#define link1FeedbackPin 50
#define link1PWMPin 13
#define link1DirPin 15
// link 2
#define link2FeedbackPin 49
#define link2PWMPin 12
#define link2DirPin 16
// jaw 1
#define jaw1FeedbackPin 47
#define jaw1PWMPin 11
#define jaw1DirPin 17
// jaw 2
#define jaw2FeedbackPin 46
#define jaw2PWMPin 10
#define jaw2DirPin 18
// wrist
#define wrist1FeedbackPin 45
#define wrist1PWMPin 9
#define wrist1DirPin 19
// roll
#define rollFeedbackPin 44
#define rollPWMPin 8
#define rollDirPin 20
// prism
#define prismFeedbackPin 42
#define prismPWMPin 7
#define prismDirPin 21
double kp = 1 , ki = 0 , kd = 0; // modify for optimal performance
double link1Input = 0, link1Output = 0, link1Setpoint = 0;
double link2Input = 0, link2Output = 0, link2Setpoint = 0;
double jaw1Input = 0, jaw1Output = 0, jaw1Setpoint = 0;
double jaw2Input = 0, jaw2Output = 0, jaw2Setpoint = 0;
double wristInput = 0, wristOutput = 0, wristSetpoint = 0;
double rollInput = 0, rollOutput = 0, rollSetpoint = 0;
double prismInput = 0, prismOutput = 0, prismSetpoint = 0;
PID link1PID(&link1Input, &link1Output, &link1Setpoint, kp, ki, kd, DIRECT);
PID link2PID(&link2Input, &link2Output, &link2Setpoint, kp, ki, kd, DIRECT);
PID jaw1PID(&jaw1Input, &jaw1Output, &jaw1Setpoint, kp, ki, kd, DIRECT);
PID jaw2PID(&jaw2Input, &jaw2Output, &jaw2Setpoint, kp, ki, kd, DIRECT);
PID wristPID(&wristInput, &wristOutput, &wristSetpoint, kp, ki, kd, DIRECT);
PID rollPID(&rollInput, &rollOutput, &rollSetpoint, kp, ki, kd, DIRECT);
PID prismPID(&prismInput, &prismOutput, &prismSetpoint, kp, ki, kd, DIRECT);
EncoderReader link1;
EncoderReader link2;
EncoderReader jaw1;
EncoderReader jaw2;
EncoderReader wrist;
EncoderReader roll;
EncoderReader prism;
int ledPin = 13;
// int clkPin = 52;
// int csPin = 53;
// int rollPin = 44;
// int jaw1Pin = 47;
// int jaw2Pin = 46;
// int wristPin = 45;
// int PWMPin_jaw1 = 11;
// int PWMPin_jaw2 = 10;
// int PWMPin_wrist = 9;
// int dirPin_jaw1 = 17;
// int dirPin_jaw2 = 18;
// int dirPin_wrist = 19;
boolean ledState = false;
void ledStatus (){
digitalWrite(ledPin, ledState = !ledState);
}
void encoderRead(){
link1.reading = link1.ReadAbsoluteAngle();
link2.reading = link2.ReadAbsoluteAngle();
jaw1.reading = jaw1.ReadAbsoluteAngle();
jaw2.reading = jaw2.ReadAbsoluteAngle();
wrist.reading = wrist.ReadAbsoluteAngle();
roll.reading = roll.ReadAbsoluteAngle();
prism.reading = prism.ReadAbsoluteAngle();
}
void setup() {
Serial.begin(115200);
link1.Setup(clkPin, csPin, link1FeedbackPin);
link2.Setup(clkPin, csPin, link2FeedbackPin);
jaw1.Setup(clkPin, csPin, jaw1FeedbackPin);
jaw2.Setup(clkPin, csPin, jaw2FeedbackPin);
wrist.Setup(clkPin, csPin, wrist1FeedbackPin);
roll.Setup(clkPin, csPin, rollFeedbackPin);
prism.Setup(clkPin, csPin, prismFeedbackPin);
link1PID.SetMode(AUTOMATIC);
link1PID.SetSampleTime(1);
link1PID.SetOutputLimits(-255, 255);
link2PID.SetMode(AUTOMATIC);
link2PID.SetSampleTime(1);
link2PID.SetOutputLimits(-255, 255);
jaw1PID.SetMode(AUTOMATIC);
jaw1PID.SetSampleTime(1);
jaw1PID.SetOutputLimits(-255, 255);
jaw2PID.SetMode(AUTOMATIC);
jaw2PID.SetSampleTime(1);
jaw2PID.SetOutputLimits(-255, 255);
wristPID.SetMode(AUTOMATIC);
wristPID.SetSampleTime(1);
wristPID.SetOutputLimits(-255, 255);
rollPID.SetMode(AUTOMATIC);
rollPID.SetSampleTime(1);
rollPID.SetOutputLimits(-255, 255);
prismPID.SetMode(AUTOMATIC);
prismPID.SetSampleTime(1);
prismPID.SetOutputLimits(-255, 255);
// pinMode(ledPin, OUTPUT);
pinMode(link1PWMPin, OUTPUT);
pinMode(link1DirPin, OUTPUT);
digitalWrite(link1DirPin, HIGH);
pinMode(link2PWMPin, OUTPUT);
pinMode(link2DirPin, OUTPUT);
digitalWrite(link2DirPin, HIGH);
pinMode(jaw1PWMPin, OUTPUT);
pinMode(jaw1DirPin, OUTPUT);
digitalWrite(jaw1DirPin, HIGH);
pinMode(jaw2PWMPin, OUTPUT);
pinMode(jaw2DirPin, OUTPUT);
digitalWrite(jaw2DirPin, HIGH);
pinMode(wrist1PWMPin, OUTPUT);
pinMode(wrist1DirPin, OUTPUT);
digitalWrite(wrist1DirPin, HIGH);
pinMode(rollPWMPin, OUTPUT);
pinMode(rollDirPin, OUTPUT);
digitalWrite(rollDirPin, HIGH);
pinMode(prismPWMPin, OUTPUT);
pinMode(prismDirPin, OUTPUT);
digitalWrite(prismDirPin, HIGH);
Timer1.attachInterrupt(encoderRead).setFrequency(340).start();
// Timer2.attachInterrupt(ledStatus).setFrequency(2).start();
}
void turnClockWise(int dirPin) {
digitalWrite(dirPin, HIGH);
}
void turnCounterClockWise(int dirPin) {
digitalWrite(dirPin, LOW);
}
void pwmOut(int pidOutput, int dirPin, int PWMPin){
if (pidOutput > 0)
{
turnClockWise(dirPin);
analogWrite(PWMPin, pidOutput);
// digitalWrite(ledPin, HIGH);
} else
{
turnCounterClockWise(dirPin);
analogWrite(PWMPin, abs(pidOutput));
// digitalWrite(ledPin, LOW);
}
}
void loop(){
if (Serial.available() > 0) {
int i = Serial.parseInt();
link1Setpoint = i;
}
link1Input = link1.reading / 21;
wristPID.Compute();
jaw1PID.Compute();
pwmOut(wristOutput, wrist1DirPin, wrist1PWMPin);
Serial.print("link1: ");
Serial.print(link1.reading);
Serial.print(" link2: ");
Serial.print(link2.reading);
Serial.print(" jaw1: ");
Serial.print(jaw1.reading/21);
Serial.print(" jaw2: ");
Serial.print(jaw2.reading/21);
Serial.print(" wrist: ");
Serial.print(wrist.reading/21);
Serial.print(" roll: ");
Serial.println(roll.reading/21);
}
What stages? Currently, we are able to position the motors at the desired angle using PID controllers. The problem we have right now is that the motors spin during sketch upload or if the reset button of the arduino due is held (it stops when the reset button is released).
Wawa:
The Due is 3.3volt logic...
Yes. But the arduino due has an on-board regulator that provides a 5V output. We only use this to power up the encoders. We are primarily powering up the arduino using the USB port.
