I am using 2 HX5010 180 degrees rotation motors for my first prototype of modular robotics.
In my project there are cubes which stick to each other and rotate against each other. I am using hall sensors to detect the orientation as well as the rotation.
The motors rotate fine only problem is I am not able to stop the motors in one position. I am unable to find where I am going wrong. If somthing could be done using code or the place ment of sensors are wrong.
Here is my code
#include <Servo.h>
Servo myservo1;
Servo myservo2;
//Servo myservo3;
const int buttonPin1 = 2; // Axthe pin that the pushbutton is attached to
const int buttonPin1a = 3; //Bx
const int buttonPin2 = 4; //Ay
const int buttonPin2a = 5; //By
//const int buttonPin3 = 6; //Az
//const int buttonPin3a =7 ; //Bz
const int serPin1 = 9; // the pin that the servo is attached to
const int serPin2 = 10;
//const int serPin3 = 11;
//int lbs1 = 0; // previous state of the button
//int lbse2 = 0;
//int lbs1a = 0;
//int lbs2a = 0;
int bs1 = 0; // current state of the button
int bs2 = 0; // current state of the button
int bs2a = 0;
int bs1a = 0;
//int bs3 = 0;
//int bs3a = 0;
int val= 180;
int val1= 0;
int temp;
int pos=0;
void setup() {
myservo1.attach(9);
myservo2.attach(10);
// myservo3.attach(11);
// initialize the button pin as a input:
pinMode(buttonPin1, INPUT);
pinMode(buttonPin1a, INPUT);
pinMode(buttonPin2, INPUT);
pinMode(buttonPin2a, INPUT);
// pinMode(buttonPin3, INPUT);
// pinMode(buttonPin3a, INPUT);
// // initialize the LED as an output:
pinMode(serPin1, OUTPUT);
pinMode(serPin2, OUTPUT);
// pinMode(serPin3, OUTPUT);
// initialize serial communication:
Serial.begin(9600);
}
void loop()
{
myservo1.attach(9);
myservo2.attach(10);
// myservo3.attach(11);
// digitalWrite(buttonPin1, HIGH); // pull-up
// digitalWrite(buttonPin1a, HIGH);
// digitalWrite(buttonPin2, HIGH);
// digitalWrite(buttonPin2a, HIGH);
bs1= digitalRead(buttonPin1); // LOW = 0 HIGH = 1
bs1a = digitalRead(buttonPin1a);
bs2 = digitalRead(buttonPin2);
bs2a = digitalRead(buttonPin2a);
// bs3 = digitalRead(buttonPin3);
// bs3a = digitalRead(buttonPin3a);
unsigned int keystate = 0;
if (bs1 == HIGH) bitSet(keystate , 0); // 1 - 1 (BIN)
if (bs1a == HIGH) bitSet(keystate , 1); // 2 - 10
if (bs2 == HIGH) bitSet(keystate , 2); // 4 - 100
if (bs2a == HIGH) bitSet(keystate , 3);// 8- 1000
// if (bs3 == HIGH) bitSet(keystate , 4); // 16- 10000
// if (bs3a == HIGH) bitSet(keystate , 5);// 32 - 100000
//
delay(1000);
// Serial.print("STATE: ");
// Serial.println(keystate, BIN); // 0-15 = 0 - 1111
temp= val;
val = val1;
val1 = temp;
switch (keystate )
{
case 1: // bs1 pressed others not
myservo1.write(val1); // tell servo to go to position in variable 'pos'
delay(1500); // waits for the servo to reach the position
Serial.println("Ax");
myservo1.detach();
delay(1500);
break;
case 2: // bs1a pressed others not
myservo1.write(val1);
delay(1500);
Serial.println("B-x");
myservo1.detach();
break;
case 4 : // bs2 pressed others not
myservo2.write(val1);
delay(1000);
Serial.println("Ay");
myservo2.detach();
break;
case 8: // bs2a pressed others not
myservo2.write(val1);
delay(1000);
Serial.println("B-y");
myservo2.detach();
break;
case 5: // bs1 and bs2 pressed others not
myservo1.write(val1);
delay(1000);
Serial.println("AA");
myservo2.write(val1);
delay(1000);
myservo1.detach();
break;
case 10: // bs1 and bs2 pressed others not
myservo1.detach();
myservo2.detach();
delay(1000);
Serial.println("BB");
break;
case 9: // bs1-Ax and bs2a-By pressed others not
Serial.println("AxBy");
myservo2.write(val1);
delay(1500);
// myservo1.write(val1);
// delay(1500);
Serial.println("AxAy");
myservo1.detach();
myservo2.detach();
break;
case 6: // bs1a-Bx and bs2-Ay pressed others not
Serial.println("BxAy");
myservo1.write(val1);
delay(1500);
// myservo2.write(val1);
// delay(1500);
Serial.println("AxAy");
myservo1.detach();
myservo2.detach();
break;
case 0: // no keys pressed;
break;
default: // invalid key combination
// error message
Serial.println("Invalid key combination pressed");
break;
myservo1.detach();
myservo2.detach();
}
}
I want to know if my approach is correct. If not please let me know where am i going wrong.
