Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <MAX14870MotorShield.h>
- #include <stdio.h>
- #include <Servo.h>
- Servo myservo; // create servo object to control a servo
- int closee = 52;
- int openn = 65 + closee;
- int sensorPin = A0; // select the input pin for LDR
- int sensorValue = 0; // variable to store the value coming from the sensor
- bool opened = false;
- int sensor_thresh = 750;
- int PWM1 = 10, DIR1 = 11, FLT1 = 12, EN1 = 13;
- int PWM2 = 3, DIR2 = 5, FLT2 = 4, EN2 = 6;
- // motor-shield - Version: Latest
- int relay1 = 8, relay2 = 2;
- MAX14870MotorShield motor1, motor2;
- String str;
- int vr, vl, ms;
- int curr;
- void setup(){
- Serial.begin(9600);
- motor1 = MAX14870MotorShield(DIR1,PWM1,EN1,FLT1);
- motor2 = MAX14870MotorShield(DIR2,PWM2,EN2,FLT2);
- myservo.attach(9); // attaches the servo on pin 9 to the servo object
- myservo.write(closee);
- pinMode(relay1,OUTPUT);
- pinMode(relay2,OUTPUT);
- motor1.setSpeed(0);
- motor2.setSpeed(0);
- batteryToDriver();
- delay(5000);
- }
- void loop(){
- if(Serial.available() < 12){
- sensorValue = analogRead(sensorPin); // read the value from the sensor
- if ((sensorValue < sensor_thresh) & (opened == false)) {
- delay(1000);
- myservo.write(openn);
- opened = true;
- }
- if ((sensorValue > sensor_thresh) & (opened == true)) {
- delay(250);
- myservo.write(closee);
- opened = false;
- }
- }
- else {
- curr = Serial.read() - 48;
- vr = 100 * curr;
- curr = Serial.read() - 48;
- vr += 10 * curr;
- curr = Serial.read() - 48;
- vr += curr;
- curr = Serial.read() - 48;
- vl = 100 * curr;
- curr = Serial.read() - 48;
- vl += 10 * curr;
- curr = Serial.read() - 48;
- vl += curr;
- curr = Serial.read() - 48;
- ms = 1000 * curr;
- curr = Serial.read() - 48;
- ms += 100 * curr;
- curr = Serial.read() - 48;
- ms += 10 * curr;
- curr = Serial.read() - 48;
- ms += curr;
- vr = vr - 500;
- vl = vl - 500;
- Serial.println(vl);Serial.println(vr);Serial.println(ms);
- int x = Serial.read() - 48;
- int y = Serial.read() - 48;
- if (x == 1 && y == 1) {
- motor1.setSpeed(vr);
- motor2.setSpeed(vl);
- delay(ms+50);
- //motor1.setSpeed(0);
- //motor2.setSpeed(0);
- delay(30);
- }
- else {
- for(int i = 0; i<100; i++){
- int j = Serial.read();
- }
- }
- }
- //motor1.setSpeed(0);
- //motor2.setSpeed(0);
- delay(30);
- }
- void batteryToDriver(){
- motor1.setSpeed(0);
- motor2.setSpeed(0);
- setRelay2(false);
- delay(50);
- setRelay1(false);
- delay(50);
- }
- void driverToBattery(){
- motor1.setSpeed(0);
- motor2.setSpeed(0);
- setRelay1(true);
- delay(50);
- setRelay2(true);
- delay(50);
- }
- void setRelay1(bool amir){
- if(amir){
- digitalWrite(relay1,LOW);
- }
- else{
- digitalWrite(relay1,HIGH);
- }
- }
- void setRelay2(bool amir){
- if(amir){
- digitalWrite(relay2,HIGH);
- }
- else{
- digitalWrite(relay2,LOW);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement