Hi,
Im trying to put this code on my arduino and I cant get it to compile. I have found and installed NewPing and fixed the first one. I cant find anywhere on how to install servo in, servo out and timer1.
Can someone please help me out? Thank you soooo much.
/code
#include<NewPing.h>
#include <ServoIn.h>
#include <ServoOut.h>
#include <Timer1.h>
#define Setpoint 80
#define Kp 2
#define respLimit 200
uint16_t RCIn[2];
uint8_t worker[SERVOIN_WORK_SIZE(2)];
uint8_t g_pinsOut[2] = {11, 12};
uint16_t RCout[2];
uint8_t g_work[SERVOOUT_WORK_SIZE(2)];
rc::ServoIn g_ServoIn(RCIn, worker, 2);
rc::ServoOut g_ServoOut(g_pinsOut, RCout, g_work, 2);
NewPing FrontSonar(A0, A1, 200);
NewPing BackSonar(A2, A3, 200);
NewPing LeftSonar(A4, A5, 200);
NewPing RightSonar(2, 3, 200);
void setup() {
Serial.begin(9600);
rc::Timer1::init();
pinMode(6, INPUT);
pinMode(7, INPUT);
pinMode(11, OUTPUT);
pinMode(12, OUTPUT);
digitalWrite(11, LOW);
digitalWrite(12, LOW);
PCMSK0 = (1 << PCINT22) | (1 << PCINT23);
PCICR = (1 << PCIE0);
RCout[0]= 1500;
RCout[1]= 1500;
g_ServoIn.start();
g_ServoOut.start();
}
void loop() {
int c1,c2,c3,c4;
int d1,d2,d3,d4;
int e1,e2,e3,e4;
int opx,opy;
g_ServoIn.update();
d1 = LeftSonar.ping_cm();
d2 = FrontSonar.ping_cm();
d3 = RightSonar.ping_cm();
d4 = BackSonar.ping_cm();
if(d1==0){d1 = 200;}
if(d2==0){d2 = 200;}
if(d3==0){d3 = 200;}
if(d4==0){d4 = 200;}
d1 = constrain(d1,0,Setpoint);
d2 = constrain(d2,0,Setpoint);
d3 = constrain(d3,0,Setpoint);
d4 = constrain(d4,0,Setpoint);
e1 = Setpoint - d1;
e2 = Setpoint - d2;
e3 = Setpoint - d3;
e4 = Setpoint - d4;
c1 = e1 * Kp;
c2 = e2 * Kp;
c3 = e3 * Kp;
c4 = e4 * Kp;
c1 = constrain(c1,0,respLimit);
c2 = constrain(c2,0,respLimit);
c3 = constrain(c3,0,respLimit);
c4 = constrain(c4,0,respLimit);
opx = d1 - d3;
opy = d2 - d4;
if(opx != 0)
{
RCout[0] = 1500 + opx;
}
else
{
RCout[0] = RCIn[0];
}
if(opy != 0)
{
RCout[1] = 1500 + opy;
}
else
{
RCout[1] = RCIn[1];
}
g_ServoOut.update();
Serial.print(d1);
Serial.print(",");
Serial.print(d2);
Serial.print(",");
Serial.print(d3);
Serial.print(",");
Serial.print(d4);
Serial.print(",");
Serial.print(RCIn[0]);
Serial.print(",");
Serial.print(RCIn[1]);
Serial.print(",");
Serial.print(RCout[0]);
Serial.print(",");
Serial.println(RCout[1]);
}
static uint8_t lastB = 0;
ISR(PCINT0_vect)
{
uint8_t newB = PINB;
uint8_t chgB = newB ^ lastB;
lastB = newB;
if (chgB)
{
if (chgB & _BV(0))
{
g_ServoIn.pinChanged(0, newB & _BV(0));
}
if (chgB & _BV(1))
{
g_ServoIn.pinChanged(1, newB & _BV(1));
}
}
}
/code