FRSDK

#include "MeccaBrain.h"
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"

MeccaBrain servik(3);
MeccaBrain servik2(2);
RF24 radio(9,10);

#define STEP 2

byte address[][6] = {"1Node","2Node","3Node","4Node","5Node","6Node"};
byte data;

const int leftSpeedPin = 6;
const int rightSpeedPin = 5;

const int leftDirPin = 7;
const int rightDirPin = 4;

const int speed = 255;

void setup() {
  Serial.begin(9600);
  Serial.write("Loading... ");

  radio.begin();
  radio.setAutoAck(1);
  radio.setRetries(0,15);
  radio.enableAckPayload();
  radio.setPayloadSize(32);

  radio.openReadingPipe(1,address[0]);
  radio.setChannel(0x60);

  radio.setPALevel (RF24_PA_MAX);
  radio.setDataRate (RF24_250KBPS);

  radio.powerUp();
  radio.startListening();

  pinMode(5, OUTPUT);
  pinMode(leftSpeedPin, OUTPUT);
  pinMode(rightSpeedPin, OUTPUT);
  pinMode(leftDirPin, OUTPUT);
  pinMode(rightDirPin, OUTPUT);
  for (int i=0; i<75; i++) {
    servik.communicate();
}
Calm();

//goForward(speed);
}

void loop()
{
  data = 0;
  data = GetRadioData();
  Serial.println(data);
  if (data == 1)  goForward(255);
  if (data == 2)  goBackward(255);
  if (data == 3)  rotateLeft(255);
  if (data == 4)  rotateRight(255);
  if (data == 0)  stopGo();
 if(Serial.available())
{
  int data = Serial.read();
  if (data == 'A') Greetings();
  if (data == 'B') MilitaryGreeting();
}
delay(50);
}

byte GetRadioData()
{
   byte pipeNo, gotByte;
   while( radio.available(&pipeNo))
   {
    radio.read( &gotByte, sizeof(gotByte));
    return gotByte;
   }
   return gotByte;

}

void stopGo()
{
  analogWrite(leftSpeedPin, 0);
  analogWrite(rightSpeedPin, 0);
  digitalWrite(leftDirPin, LOW);
  digitalWrite(rightDirPin, LOW);
}

void goForward(int speed)
{
  //Serial.write("Motor going");
  analogWrite(leftSpeedPin, speed);
  analogWrite(rightSpeedPin, speed);
  digitalWrite(leftDirPin, HIGH);
  digitalWrite(rightDirPin, HIGH);
}

void goBackward(int speed)
{
  analogWrite(leftSpeedPin, speed);
  analogWrite(rightSpeedPin, speed);
  digitalWrite(leftDirPin, LOW);
  digitalWrite(rightDirPin, LOW);
}

void rotateLeft(int speed)
{
  analogWrite(leftSpeedPin, speed);
  analogWrite(rightSpeedPin, speed);
  digitalWrite(leftDirPin, LOW);
  digitalWrite(rightDirPin, HIGH);
}

void rotateRight(int speed)
{
  analogWrite(leftSpeedPin, speed);
  analogWrite(rightSpeedPin, speed);
  digitalWrite(leftDirPin, HIGH);
  digitalWrite(rightDirPin, LOW);
}

void Calm()
{
  Serial.write("Calibrating 1st servo... ");
for(int i = 0; i <= 0; i++)
{
  servik.communicate();
  servik.setServoPosition(0,i);
}
Serial.write("Calibrating 2nd servo... ");
for(int i = 0; i <= 215; i+=10)
{
  servik.communicate();
  servik.setServoPosition(1,i);
}
Serial.write("Calibrating 3rd servo... ");
for(int i = 0; i <= 0; i+=10)
{
  servik.communicate();
  servik.setServoPosition(2,i);
}

Serial.write("Calibrating done ");
}


void Greetings()
{
  Serial.write("Greeting... ");
for(int i = 0; i <= 119; i+=10)
{
  servik2.communicate();
  servik2.setServoPosition(0,i);
}

for(int i = 0; i <= 130; i+=10)
{
  servik2.communicate();
  servik2.setServoPosition(1,i);
}

for(int i = 0; i <= 180; i+=10)
{
  servik2.communicate();
  servik2.setServoPosition(2,i);
}

for(int i = 180; i >= 100; i-=10)
{
  servik2.communicate();
  servik2.setServoPosition(2,i);
}

for(int i = 0; i <= 180; i+=10)
{
  servik2.communicate();
  servik2.setServoPosition(2,i);
}

for(int i = 180; i >= 100; i-=10)
{
  servik2.communicate();
  servik2.setServoPosition(2,i);
}

for(int i = 0; i <= 180; i+=10)
{
  servik2.communicate();
  servik2.setServoPosition(2,i);
}




for(int i = 119; i >= 0; i-=10)
{
  servik2.communicate();
  servik2.setServoPosition(0,i);
}

for(int i = 130; i <= 215; i+=10)
{
  servik2.communicate();
  servik2.setServoPosition(1,i);
}

for(int i = 180; i >=0; i-=10)
{
  servik2.communicate();
  servik2.setServoPosition(2,i);
}

}

void MilitaryGreeting()
{
  Serial.write("Millitary Greeting... ");
for(int i = 0; i <= 117; i+=9)
{
  servik.communicate();
  servik.setServoPosition(0,i);
}

for(int i = 0; i <= 80; i+=10)
{
  servik.communicate();
  servik.setServoPosition(1,i);
}

for(int i = 0; i <= 190; i+=10)
{
  servik.communicate();
  servik.setServoPosition(2,i);
}


delay(1000);

for(int i = 119; i >= 0; i-=10)
{
  servik.communicate();
  servik.setServoPosition(0,i);
}

for(int i = 130; i <= 215; i+=10)
{
  servik.communicate();
  servik.setServoPosition(1,i);
}

for(int i = 180; i >=0; i-=10)
{
  servik.communicate();
  servik.setServoPosition(2,i);
}


//Calm();
}