Combining both my servo motor code and my encoder code in arduino. Example so wh
ID: 3864827 • Letter: C
Question
Combining both my servo motor code and my encoder code in arduino. Example so when the angle of my encoder is between 40-80 degrees it jumps to the servo motor code and runs. If it gets outside of that range servo motor stops.
//code must include Servo.h library in order to work
//wanted servo library for testing of motor
//Servo
//orange pin9
//brown gnd
//red 5v
//Led
//pwm8
//Encoder
//red power 5v
//black ground
//green pin 2
//white pin 3
#include <Servo.h>
Servo myservo;
//for project encoder has 2 outputs
volatile unsigned int encoderPosition = 0; //This variable will increase or decrease depending on the rotation of encoder
float angle = 0.0;
int angle2= 0.0;
void setup() {
Serial.begin (9600); //start location
//inputs
pinMode(2, INPUT); // sets pin 2 to an input
pinMode(3, INPUT); // sets pin 3 to an input
//outputs
pinMode(8, OUTPUT); //sets pin 8 to an input
digitalWrite(2, HIGH); // sets pin 2 to a high input
digitalWrite(3, HIGH); // sets pin 3 to a high input
digitalWrite(8, LOW); //Sets pin 8 to a low input
//Setting up interrupt
attachInterrupt(0, encA, RISING); //on pulse of encoderA input with the rising edge PWM 2
attachInterrupt(1, encB, RISING); ////on pulse of encoderB input with the rising edge PWM 3
}
void loop(){
Serial.println (encoderPosition, DEC);
angle = encoderPosition * (6.0/20.0); //????according to math though should be (3.0/20.0) though requires two rotations equatls 360 degg
Serial.println (angle); //displays angle of encoder in degrees
}
//For first outputA
void encA(){
//Channel A going from low to high edge
if (digitalRead(2) == HIGH) {
// check channel B to see which way encoder is turning
if (digitalRead(3) == LOW) {
encoderPosition = encoderPosition + 1; //Turning clockwise
}
else {
encoderPosition = encoderPosition - 1; //Turning counter clockwise
}
}
else // must be a high-to-low edge on channel A
{
// check channel B to see which way encoder is turning
if (digitalRead(3) == HIGH) {
encoderPosition = encoderPosition + 1; //encoder truning clockwise
}
else {
encoderPosition = encoderPosition - 1; //encoder turning coutner clockwise
}
}
}
//For second output B
void encB(){
//Channel B going from low to high edge
if (digitalRead(3) == HIGH) {
// check channel A to see which way encoder is turning
if (digitalRead(2) == HIGH) {
encoderPosition = encoderPosition + 1; //encoder truning clockwise
}
else {
encoderPosition = encoderPosition - 1; //encoder counter truning clockwise
}
}
// Look for a high-to-low on channel B
else {
// check channel B to see which way encoder is turning
if (digitalRead(2) == LOW) {
encoderPosition = encoderPosition + 1; //encoder turuning clockwise
}
else {
encoderPosition = encoderPosition - 1; //encoder counter truning clockwise
}
}
}
void servoloop(){
//angle = constrain(angle, 0, 80);
//angle = constrain(angle, 100, 140);
//angle = constrain(angle, 160, 200);
//angle = constrain(angle, 220, 2600);
//angle = constrain(angle, 280, 320);
//angle = constrain(angle, 340, 360);
for (angle2=0; angle2<120; angle2+=1)
//goes from 0 to 120 degrees in steps of 1 degree
{
myservo.write(angle2); //directs servo to go to position angle
delay(20); //waits 20ms between servo commands
}
digitalWrite(8, HIGH); //Sets pin 8 to a high input
delay(5000);
digitalWrite(8, LOW); //Sets pin 8 to a low input
for (angle2=120; angle2>=1; angle2-=1) //goes from 120 to 0 degrees
{
myservo.write(angle2); //moves servo back in opposite direction
delay(20); //pwm 20ms
}
delay(10000); //wait for 10 seconds
}
Explanation / Answer
#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);
}
}
}
PROGRAM2:
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]);}
}}}