//-------------------------------------------------------------------------------------------- //Author: Colin Riley //Date: 4-14-2015 //Project: Robotic Gopher Tortoise Scope //Sponsor: Tall Timbers Research Station & Land Conservancy //Description: This program reads in a character from the serial port. This character is then // stored into the variable "cmd". Based of the value of this variable, this code // can perform a variety of tasks. These tasks are outlined in the table below. // Note that the decimal value of ascii characters is used rather than the actual // characters. // // ==================================================================== // | COMMAND ACTION | COMMAND ACTION | // |--------------------------------|---------------------------------| // | 96 read temp/humid | 107 slow backward | // | 97 pan + | 108 slow backward, right | // | 98 pan - | 110 fast right | // | 99 center pan | 111 fast forward right | // | 100 breaks | 112 fast forward | // | 101 slow, right | 113 fast forward left | // | 102 slow forward, right | 114 fast left | // | 103 slow forward | 115 fast backward left | // | 104 slow forward, left | 116 fast backward | // | 105 slow left | 117 fast backward, right | // | 106 slow backward left | | // ==================================================================== // // The purpose of this code is to control a rover which is carrying a temperature // and humidity sensor as well as a pan system for a cammera. This code was creted // for use with an Arduino Micro, dual H-bridge motor driver and a DHT22 humidity // and temperature sensor. // // Updates: In this version of the code a watch dog timer was included. If the arduino does // not read in a new command at least once every three seconds then it will apply // the breaks until a new command has been sent. This protects the user and rover // in the case of a communications failure. // //-------------------------------------------------------------------------------------------- //library and setup for servo control #include Servo myservo; //library and setup for the DHT22 temp and humidity sensor #include "DHT.h" #define DHTTYPE DHT22 unsigned long prev_cmd_time = 0; //time last command was received const unsigned long dog_time = 3000; //set watch dog timer to 3 seconds /*millis() gives the number of milliseconds since the arduino powered on. it overflows after approximatley 50 days. */ //variable to hold angle of the servo int servo_angle = 90; /*NOTE*/ /*Pins 9 and 10 can only be used for servos due to the requirements*/ /*of the servo.h library. */ //pin set up for DHT22 sensor int dhtpin=2; int servopin=9; //pin set up for motor controller //Left motor (green block on motor driver) int pinI1=8;//define I1 port, blue wire on ribbon cable int pinI2=7;//define I2 port, green wire int pinEA=6;//define EA(PWM speed regulation)port, yellow wire //Right motor (red block on motor driver) int pinI3=5;//define I3 port, orange wire int pinI4=4;//define I4 port, red wire int pinEB=3;//define EB(PWM spped regulation) port, brown wire //speed constants for motor control const int zero_speed=10; const int quarter_speed=60;//standard motor speeds const int half_speed=120; const int three_quarter_speed=180; const int full_speed=240; const int pause=100;//standize length of delays //servo control constants const int pan_increment=22; const int pan_center=88; //Initialize DHT sensor for normal 16mhz Arduino DHT dht(dhtpin, DHTTYPE); //variables used for recieving and storing serial data int cmd=0; int prev_cmd=0; //-------------------------------------------------------------------------------------------- void setup() { Serial.begin (9600);//define baud rate at 9600 bps for serial comms dht.begin(); //intialize temp and humidity sensor myservo.attach(servopin); //use pin 9 for servo control data //set motor controller pins as outputs pinMode(pinI1,OUTPUT); pinMode(pinI2,OUTPUT); pinMode(pinI3,OUTPUT); pinMode(pinI4,OUTPUT); pinMode(pinEA,OUTPUT); pinMode(pinEB,OUTPUT); //print start-up message //Serial.println("Arduino Ready!"); }//end setup //-------------------------------------------------------------------------------------------- //This is the main loop void loop() { //Read and store input command if (Serial.available()>0) {//if data is being received prev_cmd=cmd; //store previous command in an int cmd= Serial.read();//read in new byte and stores it as an int prev_cmd_time = millis(); //store time command was received } if ((millis() - prev_cmd_time) < dog_time){ //if time since last command is less than 3 seconds switch (cmd){ case 96: temp(); break; case 97: servo_angle += pan_increment; set_servo(servo_angle); break; case 98: servo_angle -= pan_increment; set_servo(servo_angle); break; case 99: servo_angle = pan_center; set_servo(servo_angle); break; case 100: breaks(); break; case 101: slow_right(); break; case 102: slow_forward_right(); break; case 103: slow_forward(); break; case 104: slow_forward_left(); break; case 105: slow_left(); break; case 106: slow_backward_left(); break; case 107: slow_backward(); break; case 108: slow_backward_right(); break; case 110: fast_right(); break; case 111: fast_forward_right(); break; case 112: fast_forward(); break; case 113: fast_forward_left(); break; case 114: fast_left(); break; case 115: fast_backward_left(); break; case 116: fast_backward(); break; case 117: fast_backward_right(); break; default: breaks(); }//end swich statement }//end if statement else{ //if no command has been received in the last 3 seconds breaks(); //the rover will apply the breaks } }//end of main loop //-------------------------------------------------------------------------------------------- //This function reads in temperature and humidity data from a DHT22 sensor. It then prints //the humidity, temperature in celcius and temperature in ferenheight. void temp (){ // Reading temperature or humidity takes about 250 milliseconds! // Sensor readings may also be up to 2 seconds 'old' (its a very slow sensor) float h = dht.readHumidity(); // Read temperature as Celsius float t = dht.readTemperature(); // Read temperature as Fahrenheit float f = dht.readTemperature(true); // Check if any reads failed and exit early (to try again). if (isnan(h) || isnan(t) || isnan(f)) { Serial.println("Failed to read from DHT sensor!"); return; } //print humidity Serial.print(h); Serial.print(","); //print temperature Serial.print(t); Serial.print(","); Serial.print(f); Serial.print(","); cmd=prev_cmd;//reset input command to previous command so motor control isn't interupted }//end of function "temp" //-------------------------------------------------------------------------------------------- //This function takes in an integer. It then checks that the integer is within range, and //reset the integer if neccessary. Finally, it uses the integer to position a servo motor. void set_servo(int servo_angle){ int upper_limit = 128; int lower_limit = 40; if (servo_angle > (upper_limit - 1)) { servo_angle = upper_limit; } else if (servo_angle < (lower_limit + 1)) { servo_angle = lower_limit; } myservo.write(servo_angle); cmd=prev_cmd;//reset input command to previous command so motor control isn't interupted }//end of function "set_servo" //-------------------------------------------------------------------------------------------- //This function applies the breaks void breaks (){ analogWrite(pinEA,0);//set motor A speed to zero analogWrite(pinEB,0);//set motor B speed to zero //set motor A digitalWrite(pinI1,HIGH);// DC motor stop rotating digitalWrite(pinI2,HIGH); //set motor B digitalWrite(pinI3,HIGH);// DC motor stop rotating digitalWrite(pinI4,HIGH); }//end of function "breaks" //-------------------------------------------------------------------------------------------- //This function moves the rover slowly to the right void slow_right(){ //set motor A digitalWrite(pinI1, LOW); //CW rotation digitalWrite(pinI2, HIGH); analogWrite(pinEA, half_speed); //set motor B digitalWrite(pinI3, HIGH); //CCW rotation digitalWrite(pinI4, LOW); analogWrite(pinEB, half_speed); }//end of function "slow_right" //-------------------------------------------------------------------------------------------- //This function moves the rover slowly to the forward, right void slow_forward_right(){ //set motor A digitalWrite(pinI1, LOW); //CW rotation digitalWrite(pinI2, HIGH); analogWrite(pinEA, three_quarter_speed); //set motor B digitalWrite(pinI3, LOW); //CW rotation digitalWrite(pinI4, HIGH); analogWrite(pinEB, zero_speed); }//end of function "slow_forward_right" //-------------------------------------------------------------------------------------------- //This function moves the rover slowly forward void slow_forward(){ //set motor A digitalWrite(pinI1, LOW); //CW rotation digitalWrite(pinI2, HIGH); analogWrite(pinEA, half_speed); //set motor B digitalWrite(pinI3, LOW); //CW rotation digitalWrite(pinI4, HIGH); analogWrite(pinEB, half_speed); }//end of function "slow_forward" //-------------------------------------------------------------------------------------------- //This function moves the rover slowly to the forward, left void slow_forward_left(){ //set motor A digitalWrite(pinI1, LOW); //CW rotation digitalWrite(pinI2, HIGH); analogWrite(pinEA, zero_speed); //set motor B digitalWrite(pinI3, LOW); //CW rotation digitalWrite(pinI4, HIGH); analogWrite(pinEB, three_quarter_speed); }//end of function "slow_forward_left" //-------------------------------------------------------------------------------------------- //This function moves the rover slowly to the left void slow_left(){ //set motor A digitalWrite(pinI1, HIGH); //CCW rotation digitalWrite(pinI2, LOW); analogWrite(pinEA, half_speed); //set motor B digitalWrite(pinI3, LOW); //CW rotation digitalWrite(pinI4, HIGH); analogWrite(pinEB, half_speed); }//end of function "slow_left" //-------------------------------------------------------------------------------------------- //This function moves the rover slowly to the backward, left void slow_backward_left(){ //set motor A digitalWrite(pinI1, HIGH); //CCW rotation digitalWrite(pinI2, LOW); analogWrite(pinEA, zero_speed); //set motor B digitalWrite(pinI3, HIGH); //CCW rotation digitalWrite(pinI4, LOW); analogWrite(pinEB, three_quarter_speed); }//end of function "slow_backward_left" //-------------------------------------------------------------------------------------------- //This function moves the rover slowly backward void slow_backward(){ //set motor A digitalWrite(pinI1, HIGH); //CCW rotation digitalWrite(pinI2, LOW); analogWrite(pinEA, half_speed); //set motor B digitalWrite(pinI3, HIGH); //CCW rotation digitalWrite(pinI4, LOW); analogWrite(pinEB, half_speed); }//end of function "slow_backward" //-------------------------------------------------------------------------------------------- //This function moves the rover slowly to the backward, right void slow_backward_right(){ //set motor A digitalWrite(pinI1, HIGH); //CCW rotation digitalWrite(pinI2, LOW); analogWrite(pinEA, three_quarter_speed); //set motor B digitalWrite(pinI3, HIGH); //CCW rotation digitalWrite(pinI4, LOW); analogWrite(pinEB, zero_speed); }//end of function "slow_backward_right" //-------------------------------------------------------------------------------------------- //This function moves the rover quickly to the right void fast_right(){ //set motor A digitalWrite(pinI1, LOW); //CW rotation digitalWrite(pinI2, HIGH); analogWrite(pinEA, full_speed); //set motor B digitalWrite(pinI3, HIGH); //CCW rotation digitalWrite(pinI4, LOW); analogWrite(pinEB, full_speed); }//end of function "fast_right" //-------------------------------------------------------------------------------------------- //This function moves the rover quickly to the forward, right void fast_forward_right(){ //set motor A digitalWrite(pinI1, LOW); //CW rotation digitalWrite(pinI2, HIGH); analogWrite(pinEA, full_speed); //set motor B digitalWrite(pinI3, LOW); //CW rotation digitalWrite(pinI4, HIGH); analogWrite(pinEB, quarter_speed); }//end of function "fast_forward_right" //-------------------------------------------------------------------------------------------- //This function moves the rover quickly forward void fast_forward(){ //set motor A digitalWrite(pinI1, LOW); //CW rotation digitalWrite(pinI2, HIGH); analogWrite(pinEA, full_speed); //set motor B digitalWrite(pinI3, LOW); //CW rotation digitalWrite(pinI4, HIGH); analogWrite(pinEB, full_speed); }//end of function "fast_forward" //-------------------------------------------------------------------------------------------- //This function moves the rover quickly to the forward, left void fast_forward_left(){ //set motor A digitalWrite(pinI1, LOW); //CW rotation digitalWrite(pinI2, HIGH); analogWrite(pinEA, quarter_speed); //set motor B digitalWrite(pinI3, LOW); //CW rotation digitalWrite(pinI4, HIGH); analogWrite(pinEB, full_speed); }//end of function "fast_forward_left" //-------------------------------------------------------------------------------------------- //This function moves the rover quickly to the left void fast_left(){ //set motor A digitalWrite(pinI1, HIGH); //CCW rotation digitalWrite(pinI2, LOW); analogWrite(pinEA, full_speed); //set motor B digitalWrite(pinI3, LOW); //CW rotation digitalWrite(pinI4, HIGH); analogWrite(pinEB, full_speed); }//end of function "fast_left" //-------------------------------------------------------------------------------------------- //This function moves the rover quickly to the backward, left void fast_backward_left(){ //set motor A digitalWrite(pinI1, HIGH); //CCW rotation digitalWrite(pinI2, LOW); analogWrite(pinEA, quarter_speed); //set motor B digitalWrite(pinI3, HIGH); //CCW rotation digitalWrite(pinI4, LOW); analogWrite(pinEB, full_speed); }//end of function "slow_backward_left" //-------------------------------------------------------------------------------------------- //This function moves the rover quickly backward void fast_backward(){ //set motor A digitalWrite(pinI1, HIGH); //CCW rotation digitalWrite(pinI2, LOW); analogWrite(pinEA, full_speed); //set motor B digitalWrite(pinI3, HIGH); //CCW rotation digitalWrite(pinI4, LOW); analogWrite(pinEB, full_speed); }//end of function "fast_backward" //-------------------------------------------------------------------------------------------- //This function moves the rover quickly to the backward, right void fast_backward_right(){ //set motor A digitalWrite(pinI1, HIGH); //CCW rotation digitalWrite(pinI2, LOW); analogWrite(pinEA, full_speed); //set motor B digitalWrite(pinI3, HIGH); //CCW rotation digitalWrite(pinI4, LOW); analogWrite(pinEB, quarter_speed); }//end of function "fast_backward_right" //--------------------------------------------------------------------------------------------