Hi Everyone,
I'm a newbie to C/C++ and arduino. I am trying to use an arduino Mega2056 to control 4 linear actuators. We are attaching rotary encoders on a rack and pinion that is attached to each actuator. As each actuator extends, the rotary encoder will turn this will allow me to determine exact length of each linear actuator. The linear actuators are arranged in pairs parallel to one another and are intended to move in tandom with its parallel member. For our device to work properly, we need to be constantly adjusting the actuator length as a function of the IMU pitch value. For this reason I am afraid to use a while loop to let the program hang until the actuators reach the set value, as a new length may need to be achieved before the actuator reaches the length associated with the pitch at the time of entry for the while loop. For this reason I am also afraid to use delays.
Getting to the crux of the problem:
First, is there a way that I can set a length using as I'm doing in the code below, but also adjust any misalignment that develops between each pair of arms. I've tried to do this below with EqArms, but I realized that it will run almost instantly after the the linear actuators have been sent a new position. What is the best way that I can allow for the adjustment after the actuators reach their assigned lengths, and before they begin heading to their next set point, while minimizing the delay for the next iteration?
Second, when you read from an input pin, such as for the rotary encoders, if it is still turning, will the program hang on the first instance of digitalRead(Rots[0]) until the pin stops receiving a signal, and then start trying to listen to pin the second encoder with digitalRead(Rots[1])? If I need to digitalRead four different pins to keep track of the length of each arm, is this the proper way to go about doing so? Will a single board be able to handle this?
I know this is more than one question, but hopefully I have been clear enough. I would hook this up and try it out, but our parts are on the way and our deadline is too close to not start the coding now. Please help if you can.
#include "UM7.h"
#include <Servo.h>
//#include <SoftwareSerial.h>
//Connect the RX pin on the UM7 to TX1 (pin 18) on the DUE
//Connect the TX pin on the UM7 to RX1 (pin 19) on the DUE
UM7 imu;
Servo LA1; //create servo
Servo LA2;
Servo LA3;
Servo LA4;
//#include <SoftwareSerial.h>
//SoftwareSerial serial1(0,1); // RX, TX
#define K 1
int motorPin = 9;
int velocity = 0;
int n = 0;
int time_init = 0;
int zeropoint = 0;
int average_zero = 0;
//declaring digital input pins for rotary encoders arm 1
int rot1=22;//rotary encoder actuator 1 on arm1
int rot2=23;//rotary encoder actuator 2 on arm 1
//declaring digital input pins for rotary encoders arm 2
int rot3=24;//rotary encoder actuator 3 arm 2
int rot4=25;//rotary encoder actuator 4 arm 2
int Rots[4]={rot1,rot2,rot3,rot4};
//declaring driver pwm pins
int drv1=14;//driver for actuator 1, arm 1
int drv2;//driver for actuator 2, arm 1
int drv3;//driver for actuator 3, arm 2
int drv4;//driver for actuator 4, arm 2
//declaring initial Lengths of LA's
int Length[4]={0,0,0,0};
void setup() //does not return anything
{
// put your setup code here, to run once:
//Whichever Port you initialize last will be the one that stays listenting by default;
Serial.begin(115200);//alLOWs read and write from serialmonitor inputs/USB RO, TO pins
Serial1.begin(115200);//alLOWs read and write from R1,T1 pins 4
//Serial2.begin(115200);
//Serial3.begin(115200);
time_init = millis();
//set the rotary encoder pins to receive a digital signal
pinMode(rot1,INPUT);
pinMode(rot2,INPUT);
pinMode(rot3,INPUT);
pinMode(rot4,INPUT);
//set driver pins;
pinMode(drv1,OUTPUT);
pinMode(drv2,OUTPUT);
pinMode(drv3,OUTPUT);
pinMode(drv4,OUTPUT);
LA1.attach(drv1); //attach servo (whatever PWM pin you connect the servo input to)
LA2.attach(drv2);
LA3.attach(drv3);
LA4.attach(drv4);
}
void Eqarms(int arm,int lengthx, int lengthy, int drvx, int drvy){
while (lengthx>lengthy){
digitalWrite(drvy,HIGH);
//drv1 was the pwm pin so can't just turn it on vs off now can I? Wouldn't this just be equal
//to analogWrite(motorpin,255);
while (lengthx<lengthy){
digitalWrite(drvx,HIGH);//I don't think it will be as simple as this becuase driving High will only be one direction. I'll have to swap the polarity of the positive and negative pwm
//terminals in the code?
digitalWrite(drvy,LOW);
if (arm=1){
Length[1]=Length[1]+digitalRead(Rots[1]);// it will be Rots[1]*length per period/count of periods}
}
else {Length[3]=Length[3]+digitalRead(Rots[3]);}
}}}
void SetLength(){
for(int i=0;i<4;i++){
Length[i]=Length[i]+digitalRead(Rots[i]);}}
void loop() {
// put your main code here, to run repeatedly:
if (Serial1.available() > 0) {
if (imu.encode(Serial1.read()))
{ // Reads byte from buffer. Valid packet returns true.
if (millis() - time_init > 3000)
{
velocity = K * (abs((imu.roll - average_zero) / 91.02222));
if (velocity > 254)
velocity = 254;
Serial.println(int(velocity));
Serial.println(int(average_zero/91.02222));
int val = map(velocity, 0, 254, 0, 180);
//if works with servo input
LA1.write(val);//will use servo library and specify an angle to go to...not sure will work with LA
LA2.write(val);
LA3.write(val);
LA4.write(val);
}
//Want this to run after they've both actuated to their setpoint
Eqarms(1,Length[0],Length[1],drv1,drv2);
Eqarms(2,Length[2],Length[3],drv3,drv4);
delay (5);//don't necessarilly need
}
else
{
zeropoint = imu.roll + zeropoint;
//Serial.println(int(zeropoint/92));
n = n + 1;
average_zero = zeropoint / n;
Serial.print("average_zero is" );
Serial.println(average_zero/92);
}
}
}