//Libraies
#include <SPI.h>
#include <SdFat.h>
#include <IRremote.h>
#include <Servo.h>
SdFat SD;
//Remote buttons
int play = 0; // = #
int nextStep = 0; // Arrow up
int Stop = 0; // = Star
int Rec = 0; //= OK
int writeSD=0; //Arrow down
int delAll=0; //Arrow left
int posReached =0; //All servos have reache their positions
//int stepCount = -1;
int val = HIGH; //Memory for Repeat action (Low if Repeat)
int servo1, servo2, servo3, servo4, servo5, servo6;
//int stepVal;
//int recVal;
int velo;
//Input/ outputs
int RECV_PIN = 15; //Pin select IR
int CSPin =10; //Chipselect SD
int LED=16; //LED in front
//IR
IRrecv irrecv(RECV_PIN);
decode_results results;
const int activeServos =6; //No of Servos in action
const int top=15; //Max no of storage in array
unsigned long codeValue; //Detection by IR
//define servo objects
Servo S1, S2, S3, S4, S5, S6;
int armIs[activeServos] ; // array "is" Position
int armPos[top][activeServos]; // array to hold arm positions up to defined by "top"
int posCount = 0; // to count number of positions increased when "save" button pressed
int posCountMax; // max number of positions recorded
File Robot; //Name of file for SD
//-------------------------------------------------------------------OK
void setup()
{
const int homeS1 =90 , homeS2 = 94, homeS3 = 170, homeS4 = 150, homeS5 = 68, homeS6 = 70; //Home position
pinMode(LED, OUTPUT);
Serial.begin(9600);
irrecv.enableIRIn(); // Start the receiver
// attach servos to relevent pins on arduino nano
S1.attach(2);
S2.attach(3);
S3.attach(4);
S4.attach(5);
S5.attach(6);
S6.attach(7);
//Home position when start up.
S1.write(homeS1);
S2.write(homeS2);
S3.write(homeS3);
S4.write(homeS4);
S5.write(homeS5);
S6.write(homeS6);
//SetActual Position
armIs[0]=homeS1;
armIs[1]=homeS2;
armIs[2]=homeS3;
armIs[3]=homeS4;
armIs[4]=homeS5;
armIs[5]=homeS6;
Serial.print("Initializing SD card...");
if (!SD.begin(CSPin))
{
Serial.println("initialization failed!");
return;
}
Serial.println("initialization done.");
//readSD vaules at start-up into array
Robot = SD.open("prgm.txt");
if (Robot)
{
Serial.println("prgm.txt is loaded from SD");
// read from the file until there's nothing else in it:
while (Robot.available())
{
// Serial.println(Robot.read());
for (int i =0; i<activeServos; i++)
{
armPos[posCount][i] = (Robot.read());
}
posCount= posCount+1;
}
posCountMax=posCount;
// Serial.print("posCountMax = "); Serial.println(posCountMax-1);
// close the file:
Robot.close();
}
else
{
// if the file didn't open, print an error:
Serial.println("error opening prgm.txt or file not present");
}
Serial.println("KjellFa is ready for action");
int t2= 30;
digitalWrite(LED, LOW); //Gives a "long" LED signal when action is acknowledged
delay(t2*40);
digitalWrite(LED, HIGH);
} //End SetUp
//----------------Loop---------------------------------------------------OK
void loop()
{
readIR(); //done
codeDecode(); //done
Record(); //done
playBack(); //done
stopMove(); //done
movebyIR(); //done
writetoSD(); //done
deleteAll(); //done
}
//-------------------------------------------------------------OK
void readIR()
{
if (irrecv.decode(&results))
{
codeValue = (results.value);
Serial.print("codeValue = "); Serial.println(codeValue);
delay(100);
irrecv.resume(); // Receive the next value
}
}
//-------------------------------------------------------------------OK
void codeDecode()
{
if (codeValue == 16738455) {servo1 = 12;} //"1" Servo1 Left
if (codeValue == 16750695) {servo1 = 11;} //"2" Servo1 Right
if (codeValue == 16724175) {servo2 = 22;} //"4" Servo2 Forward
if (codeValue == 16718055) {servo2 = 21;} //"5" Servo2 Backwards
if (codeValue == 16756815) {servo3 = 32;} //"3" Servo3 Up
if (codeValue == 16743045) {servo3 = 31;} //"6" Servo3 Down
if (codeValue == 16734885) {servo4 = 42;} //"9" Servo4 up
if (codeValue == 16730805) {servo4 = 41;} //"0" Servo4down
if (codeValue == 16716015) {servo5 = 51;} //"7" Servo5Left
if (codeValue == 16726215) {servo5 = 52;} //"8" Servorighr
if (codeValue == 16720605) {servo6 = 61;} //"<" Servo6 Open / Close
if (codeValue == 16761405) {delAll = 1;} //">" Delete all both SD and RAM
if (codeValue == 16754775) {writeSD = 1;} //"V" WritetoCD
if (codeValue == 16736925) {nextStep = 1;} //"A" Next step
if (codeValue == 16732845) {play = 1;} //"#" Play
if (codeValue == 16712445) {Rec = 1;} //OK position stored in arrary no
if (codeValue == 16728765) {Stop = 1;} //"*" Stop
}
//-------------------------------------------------------------------OK
void Record()
{
if (Rec == 1)
{
if(posCount<top-1)
{
for (int i =0; i<activeServos; i ++)
{
armPos[posCount][i] = armIs[i];
}
posCount = posCount + 1; posCountMax = posCount;
stopRec(); //Stop moving servos when ""Rec" button is pressed
}
blinkUp();
}
codeValue = 0; //Reset input selection
Rec = 0; //Reset routine
}
//-----------------------------------------------------------------------
void playBack()
{
int val =1; //Robot will repeat program until velo speed is below setting and val=0
if (play == 1)
{
posCount =0;
blinkUp();
while(posCount<posCountMax)
{
veloSpeed();
Read();
playServo(); // physically move arm to updated position, this is broken into small steps
delay(velo); // pause between moves
readIR();
if (codeValue == 16728765) {blinkUp(); posCount= posCountMax; val=LOW; codeValue =0; play =0;}
if(velo > 40)
{
val=LOW;
}
if ((posCount == posCountMax) && (val == HIGH))
{
posCount = 0;
}
}
}
codeValue = 0; //Reset input selection
play = 0; //Reset routine
}
//----------------------------------------------------------------------OK
void writetoSD()
{
if(writeSD==1)
{
blinkUp();
SD.remove("prgm.txt");
Serial.println("Old prgm.txt is deleted from SD");
posCount = 0;
Robot = SD.open("prgm.txt", FILE_WRITE);
if (Robot)
{
Serial.println("New prgm is written to SD :");
while(posCount< posCountMax)
{
for(int i=0; i<activeServos; i++)
{
Robot.write(armPos[posCount][i]);
// Serial.print("Step =");Serial.print(posCount); Serial.print(" Servo ");Serial.print(i); Serial.print(" Value = ");
// Serial.println(armPos[posCount][i]);
}
posCount ++;
}
// close the file:
Robot.close();
Serial.println("done.");
}
else
{
// if the file didn't open, print an error:
Serial.println("error writing to prgm.txt");
}
}
codeValue =0;
writeSD = 0;
}
//--------------------------------------------------------------------------------------OK
void deleteAll()
{
if ( delAll == 1) //Del button arraw down
{
blinkUp();
SD.remove("prgm.txt");
// Serial.println("prgm.txt is deleted from SD");
while(posCount<posCountMax)
{
for (int i=0; i<activeServos; i++)
{
armPos[posCount][i] = {00};
}
posCount ++;
}
// Serial.println("Flash memo is cleared and deleted");
posCount = 0;
posCountMax = 0;
}
codeValue = 0; //Reset input selection
delAll=0; //Reset routine
}
//----------------------------------------------------------------OK
void stopMove()
{
if (Stop == 1) //Stop button
{
blinkUp();
servo1 = servo2 = servo3 = servo4 = servo5= servo6 = 0;
codeValue = 0; //Reset input selection
Stop = 0; //reset routine
}
}
//----------------------------------------------------------------------------OK
void movebyIR()
{
const int S1min = 18, S1max = 166, S2min = 20, S2max = 175, S3min = 60, S3max = 177,
S4min = 5, S4max = 172, S5min = 2, S5max = 180, S6min = 70, S6max = 90;
const int v = 1 ; //Increments in for loops movebyIR
veloSpeed();
//Servo1
if ((servo1 == 11) && (armIs[0] > S1min))
{
blinkUp();
// Serial.print("Servo1 = "); Serial.println (armIs[0]);
armIs[0] = armIs[0] - v;
S1.write(armIs[0]);
delay(velo);
}
if ((servo1 == 12) && (armIs[0] < S1max))
{
blinkUp();
//Serial.print("Servo1 = "); Serial.println (armIs[0]);
armIs[0] = armIs[0] + v;
S1.write(armIs[0]);
delay(velo);
}
//Servo2
if ((servo2 == 21) && (armIs[1] > S2min))
{
blinkUp();
//Serial.print("Servo2 = "); Serial.println (armIs[1]);
armIs[1] = armIs[1] - v;
S2.write(armIs[1]);
delay(velo);
}
if ((servo2 == 22) && (armIs[1] < S2max))
{
blinkUp();
//Serial.print("Servo2 = "); Serial.println (armIs[1]);
armIs[1] = armIs[1] + v;
S2.write(armIs[1]);
delay(velo);
}
//Servo3
if ((servo3 == 31) && (armIs[2] > S3min))
{
blinkUp();
//Serial.print("Servo3 = "); Serial.println (armIs[2]);
armIs[2] = armIs[2] - v;
S3.write(armIs[2]);
delay(velo);
}
if ((servo3 == 32) && (armIs[2] < S3max))
{
blinkUp();
//Serial.print("Servo3 = "); Serial.println (armIs[2]);
armIs[2] = armIs[2] + v;
S3.write(armIs[2]);
delay(velo);
}
//Servo 4
if ((servo4 == 41) && (armIs[3] > S4min))
{
blinkUp();
//Serial.print("Servo4 = "); Serial.println (armIs[3]);
armIs[3] = armIs[3] - v;
S4.write(armIs[3]);
delay(velo);
}
if ((servo4 == 42) && (armIs[3] < S4max))
{
blinkUp();
//Serial.print("Servo4 = "); Serial.println (armIs[3]);
armIs[3] = armIs[3] + v;
S4.write(armIs[3]);
delay(velo);
}
//Servo 5
if ((servo5 == 51) && (armIs[4] > S5min))
{
blinkUp();
// Serial.print("Servo5 = "); Serial.println (armIs[4]);
armIs[4] = armIs[4] - v;
S5.write(armIs[4]);
delay(velo);
}
if ((servo5 == 52) && (armIs[4] < S5max))
{
blinkUp();
//Serial.print("Servo5 = "); Serial.println (armIs[4]);
armIs[4] = armIs[4] + v;
S5.write(armIs[4]);
delay(velo);
}
//Servo 6
if ((servo6 == 61) && (armIs[5] <= S6min))
{
blinkUp();
while(armIs[5] <= S6max)
{
// Serial.print("Servo6 = "); Serial.println (armIs[5]);
armIs[5]++;
S6.write(armIs[5]);
delay(velo);
}
codeValue = 0;
servo6 =0;
}
if ((servo6 == 61) && (armIs[5] >= S6max))
{
blinkUp();
while(armIs[5] >=S6min)
{
// Serial.print("Servo6 = "); Serial.println (armIs[5]);
armIs[5] --;
S6.write(armIs[5]);
delay(velo);
}
codeValue = 0;
servo6 =0;
}
}
//--------------------Sub Routine for veloSpeed------------------------------------
void veloSpeed()
{
velo = analogRead(0); //Reading potentiometer value from pin A0
velo = map(velo, 1024, 0, 50, 1); //Map the value to a good velocity value - velo
}
//-----------Sub Routine to stop move when OK is pressed for direct recording----------OK
void stopRec()
{
blinkUp();
servo1 = servo2 = servo3 = servo4 = servo5 = servo6 = 0;
}
//-------------------Sub Routine to read and move from point to point----------OK
void Read()
{
int posReached;
posReached = 0;
for (int i = 0; i < activeServos ; i++ )
{
if ( armIs[i] > armPos[posCount][i]) armIs[i] = armIs[i] - 1;
if ( armIs[i] < armPos[posCount][i]) armIs[i] = armIs[i] + 1;
if ( armIs[i] == armPos[posCount][i]) posReached++;
}
if (posReached == activeServos) posCount ++;
}
//-----------------Sub Routine to move Servos to new position-------------OK
void playServo()
{ //Move all servos to new position
S1.write(armIs[0]);
S2.write(armIs[1]);
S3.write(armIs[2]);
S4.write(armIs[3]);
S5.write(armIs[4]);
S6.write(armIs[5]);
}
//----Sub Routine to give a flash when a button is operated--------OK
void blinkUp()
{
const int t2 =80; //Length of LED Blink
digitalWrite(LED, LOW);
delay(t2); //Length of flash
digitalWrite(LED, HIGH );
}
Comments