Skip to content

Commit

Permalink
First commit
Browse files Browse the repository at this point in the history
  • Loading branch information
NikodemBartnik committed May 23, 2019
1 parent a99e483 commit 8e22684
Show file tree
Hide file tree
Showing 2 changed files with 135 additions and 0 deletions.
61 changes: 61 additions & 0 deletions Encoded motor/knob.ino
Original file line number Diff line number Diff line change
@@ -0,0 +1,61 @@
#include <PIDController.h>
volatile long int encoder_pos = 0;
PIDController pos_pid;
int motor_value = 255;

void setup() {

Serial.begin(9600);
pinMode(2, INPUT);
pinMode(3, INPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
attachInterrupt(digitalPinToInterrupt(2), encoder, RISING);

pos_pid.begin();
pos_pid.tune(20, 0, 200);
pos_pid.limit(-255, 255);
}

void loop() {
pos_pid.setpoint((int)(((float)analogRead(A0)/1023) * 374.0 *1));
motor_value = pos_pid.compute(encoder_pos);
if(motor_value > 0){
MotorCounterClockwise(motor_value);
}else{
MotorClockwise(abs(motor_value));
}
Serial.println(encoder_pos);
delay(2);
}



void encoder(){

if(digitalRead(3) == HIGH){
encoder_pos++;
}else{
encoder_pos--;
}
}

void MotorClockwise(int power){
if(power > 50){
analogWrite(9, power);
digitalWrite(10, LOW);
}else{
digitalWrite(9, LOW);
digitalWrite(10, LOW);
}
}

void MotorCounterClockwise(int power){
if(power > 50){
analogWrite(10, power);
digitalWrite(9, LOW);
}else{
digitalWrite(9, LOW);
digitalWrite(10, LOW);
}
}
74 changes: 74 additions & 0 deletions Encoded motor/serial.ino
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
#include <PIDController.h>
volatile long int encoder_pos = 0;
PIDController pos_pid;
int motor_value = 255;
unsigned int integerValue=0; // Max value is 65535
char incomingByte;
void setup() {

Serial.begin(9600);
pinMode(2, INPUT);
pinMode(3, INPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
attachInterrupt(digitalPinToInterrupt(2), encoder, RISING);

pos_pid.begin();
pos_pid.tune(15, 0, 2000);
pos_pid.limit(-255, 255);
}

void loop() {

if (Serial.available() > 0) {
integerValue = 0;
while(1) {
incomingByte = Serial.read();
if (incomingByte == '\n') break;
if (incomingByte == -1) continue;
integerValue *= 10;
integerValue = ((incomingByte - 48) + integerValue);
pos_pid.setpoint(integerValue);
}
}

motor_value = pos_pid.compute(encoder_pos);
if(motor_value > 0){
MotorCounterClockwise(motor_value);
}else{
MotorClockwise(abs(motor_value));
}
Serial.println(encoder_pos);
delay(10);
}



void encoder(){

if(digitalRead(3) == HIGH){
encoder_pos++;
}else{
encoder_pos--;
}
}

void MotorClockwise(int power){
if(power > 100){
analogWrite(9, power);
digitalWrite(10, LOW);
}else{
digitalWrite(9, LOW);
digitalWrite(10, LOW);
}
}

void MotorCounterClockwise(int power){
if(power > 100){
analogWrite(10, power);
digitalWrite(9, LOW);
}else{
digitalWrite(9, LOW);
digitalWrite(10, LOW);
}
}

1 comment on commit 8e22684

@Shreyisboss
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

thanks

Please sign in to comment.