/* Marcello Tania */ #include #include #include RF24 radio(7, 8); // CE, CSN const byte address[6] = "00001"; // Output pin float tiltRad =0 ; float panRad =0 ; const int dirPin1 = 5; const int stepPin1 = 4; const int dirPin2 = 3; const int stepPin2 = 2; const int dirPin3 = 9; const int stepPin3 = 10; int led = 6; // pin 6 for the LED float delaySpeed ; int maxDelaySpeed =500; int minDelaySpeed =100; int state=2;// initiliaze automous mode at the beginning // the setup function runs once when you press reset or power the board void setup() { // initialize serial communication Serial.begin(1000000); // use the same baud-rate as the python side while (!Serial); //Setup the nRF24L01 radio module radio.begin(); radio.openReadingPipe(0, address); radio.setPALevel(RF24_PA_MIN); radio.startListening(); // initialize digital pins as an output. pinMode(stepPin1,OUTPUT); pinMode(stepPin2,OUTPUT); pinMode(stepPin3,OUTPUT); pinMode(dirPin1,OUTPUT); pinMode(dirPin2,OUTPUT); pinMode(dirPin3,OUTPUT); pinMode(led,OUTPUT); } // the loop function runs over and over again forever void loop() { if (radio.available()) { radio.read(&state, sizeof(state)); // Serial.print("state:"); //Serial.println(state); switch(state){ case 1: digitalWrite(led, HIGH); // turn LED ON // Serial.println("manual"); break; case 2: digitalWrite(led, LOW); // turn LED ON autonomousMode(); // Serial.println("autonomous"); break; case 3://error movement(0,1, 1200); // Serial.println("up"); break; case 4: movement(0,-1, 1200); // Serial.println("down"); break; case 5: movement(1,0, 700); // Serial.println("right"); break; case 6: // Serial.println("left"); movement(-1,0, 700); break; } }else if (state==2){ //if the radio is not available go to autonomous mode autonomousMode(); } } void autonomousMode(){ if (Serial.available()) { panRad = Serial.parseFloat();// first value of the serial tiltRad = Serial.parseFloat();// second value of the serial movement(rad2deg(panRad),rad2deg(tiltRad)); } } float rad2deg(float rad){ //this function convert radian to degrees float degree = rad*57.2958; //convert rad to degrees return degree; } void movement(float panDegree, float tiltDegree){ //calculating the steps for pan and tilt int panSteps = panDegree/0.0375; int tiltSteps = tiltDegree/0.0375; //geting the right direction for pan if(panDegree <0){ digitalWrite(dirPin3,LOW); // Enables the motor to move in a particular direction panSteps = abs(panSteps); // make the stepping as a + value Serial.println("right"); }else if(panDegree>0){ digitalWrite(dirPin3,HIGH); // Enables the motor to move in a particular direction Serial.println("left"); } //geting the right direction for tilt if(tiltDegree<0){ digitalWrite(dirPin1,HIGH); // Enables the motor to move in a particular direction digitalWrite(dirPin2,LOW); // Enables the motor to move in a particular direction tiltSteps = abs(tiltSteps); // make the stepping as a + value Serial.println("moveDown"); }else if(tiltDegree>0){ digitalWrite(dirPin1,LOW); // Enables the motor to move in a particular direction digitalWrite(dirPin2,HIGH); // Enables the motor to move in a particular direction Serial.println("moveUp"); } //geting the maximum value between the panSteps and tiltSteps int maximumSteps = max(panSteps,tiltSteps); boolean panMaximum; int steps; int ratioPanTiltSteps; if(maximumSteps == panSteps){ panMaximum = true; //Serial.println("panSteps is the maximum"); steps = panSteps; if(tiltSteps |=0){ ratioPanTiltSteps= abs(panSteps/tiltSteps); // calculating the integer of the ratio because pan is bigger than tilt }else{ ratioPanTiltSteps = panSteps; } }else if(maximumSteps == tiltSteps){ panMaximum = false; steps = tiltSteps; //Serial.println("panSteps is the minimum"); if(panSteps|=0){ ratioPanTiltSteps= abs(tiltSteps/panSteps); // calculating the integer of the ratio because tilt is bigger than pan }else{ ratioPanTiltSteps= tiltSteps; } } //variable for acceleration and decelerate float fourthSteps = steps/4; float threeFourthsSteps =3/4*steps; delaySpeed = maxDelaySpeed; float deltaDelaySteps =float(maxDelaySpeed-minDelaySpeed)/fourthSteps; int subCounter = 0; // counter to wait and syncronize the slower movement of pan or tilt for(int x = 0; x < steps; x++) { //Serial.print("x :"); //Serial.println(x); if(xthreeFourthsSteps){ //accelerate half of the sum of the steps delaySpeed += deltaDelaySteps ; } if ( panMaximum == true){ digitalWrite(stepPin3,HIGH); delayMicroseconds(delaySpeed); digitalWrite(stepPin3,LOW); delayMicroseconds(delaySpeed); if( subCounter == ratioPanTiltSteps){ subCounter =0; digitalWrite(stepPin1,HIGH); digitalWrite(stepPin2,HIGH); delayMicroseconds(delaySpeed); digitalWrite(stepPin1,LOW); digitalWrite(stepPin2,LOW); } subCounter++; }else if (panMaximum == false){ digitalWrite(stepPin1,HIGH); digitalWrite(stepPin2,HIGH); delayMicroseconds(delaySpeed); digitalWrite(stepPin1,LOW); digitalWrite(stepPin2,LOW); if( subCounter == ratioPanTiltSteps){ subCounter =0; digitalWrite(stepPin3,HIGH); delayMicroseconds(delaySpeed); digitalWrite(stepPin3,LOW); delayMicroseconds(delaySpeed); } subCounter++; } } } void movement(float panDegree, float tiltDegree, float delaySpeed){ //calculating the steps for pan and tilt int panSteps = panDegree/0.0375; int tiltSteps = tiltDegree/0.0375; //geting the right direction for pan if(panDegree <0){ digitalWrite(dirPin3,LOW); // Enables the motor to move in a particular direction panSteps = abs(panSteps); // make the stepping as a + value Serial.println("right"); }else if(panDegree>0){ digitalWrite(dirPin3,HIGH); // Enables the motor to move in a particular direction Serial.println("left"); } //geting the right direction for tilt if(tiltDegree<0){ digitalWrite(dirPin1,HIGH); // Enables the motor to move in a particular direction digitalWrite(dirPin2,LOW); // Enables the motor to move in a particular direction tiltSteps = abs(tiltSteps); // make the stepping as a + value }else if(tiltDegree>0){ digitalWrite(dirPin1,LOW); // Enables the motor to move in a particular direction digitalWrite(dirPin2,HIGH); // Enables the motor to move in a particular direction } //geting the maximum value between the panSteps and tiltSteps int maximumSteps = max(panSteps,tiltSteps); boolean panMaximum; int steps; int ratioPanTiltSteps; if(maximumSteps == panSteps){ panMaximum = true; //Serial.println("panSteps is the maximum"); steps = panSteps; ratioPanTiltSteps = panSteps; }else if(maximumSteps == tiltSteps){ panMaximum = false; steps = tiltSteps; //Serial.println("panSteps is the minimum"); ratioPanTiltSteps= tiltSteps; } int subCounter = 0; // counter to wait and syncronize the slower movement of pan or tilt for(int x = 0; x < steps; x++) { if ( panMaximum == true){ digitalWrite(stepPin3,HIGH); delayMicroseconds(delaySpeed); digitalWrite(stepPin3,LOW); delayMicroseconds(delaySpeed); }else if (panMaximum == false){ digitalWrite(stepPin1,HIGH); digitalWrite(stepPin2,HIGH); delayMicroseconds(delaySpeed); digitalWrite(stepPin1,LOW); digitalWrite(stepPin2,LOW); } } }