I have a strip of leds cut into pairs; each pair is in its own lantern, and I want the lanterns to alternate whatever I put through them.
I am using FastLED library. I have posted what I have come up with. It seems to work in the console; the arrays generated are right(or seem to be)' even[] = {0,1,4,5,8,9...}; odd[] = {2,3,6,7,10,11...}' but lantern #1 does not work as the rest do for some reason.
lantern #1 = leds[even[0]] and leds[even[1]] should both display Eval color but leds[even[1]] is displaying Oval color.
the other lanterns display properly
lantern #2 = leds[odd[0]] and leds[odd[1]] displays Oval color.
lantern #3 = leds[even[2]] and leds[even[3]] displays Eval color.
lantern #4 = leds[odd[2]] and leds[odd[3]] displays Oval color.
etc...
#include <FastLED.h>
#define NUM_LEDS 12
#define DATA_PIN 3
#define POT_PIN A0
int potValue;
int even[NUM_LEDS]; //{0,1,4,5,8,9...}
int odd[NUM_LEDS]; //{2,3,6,7,10,11...}
int Epos = 0; //count position for adding lanterns to even/odd arrays
int Opos = 0;
int lantern = 1; // 1 and 2 switch back and forth between lanterns
int pos = 0; // count 2 leds per lantern
CRGB leds[NUM_LEDS];
void setup() {
Serial.begin(9600);
FastLED.addLeds<WS2812B, DATA_PIN, RGB>(leds, NUM_LEDS);
for (int i=0;i<NUM_LEDS;i++){ //go through leds
if (pos == 2){
pos = 0;
if (lantern == 1){
lantern = 2;
}
else if (lantern == 2){
lantern = 1;
}
}
switch (lantern){
case 1: // even lantern
even[Epos] = i; // i = led on strip
Epos++;
pos++;
break;
case 2: // odd lantern
odd[Opos] = i; // i = led on strip
Opos++;
pos++;
break;
}
}
Serial.print("\n even: ");
for (int x=0;x<NUM_LEDS;x++){
Serial.print(even[x]);
}
Serial.print("\n odd: ");
for (int x=0;x<NUM_LEDS;x++){
Serial.print(odd[x]);
}
}
//////////////////////////////////////////////////////////////////////////
void split(){
potValue = analogRead(POT_PIN);
int Eval;
int Oval;
int Eval = potValue; //color value for even lanterns
int Oval = map(potValue,0,1023, 10,500); //offset color value for odd lanterns
for (int i=0;i<NUM_LEDS;i++){
leds[even[i]] = CHSV(Eval,255,255);
delay(1);
leds[odd[i]] = CHSV(Oval,255,255);
}
FastLED.show();
}
///////////////////////////////////////////////////////////////////////////
void loop() {
split();
}
I am pretty new to Arduino.have I done something wrong? Is this an efficient way to go about this?
Related
I am trying to control a few (8 for now) servo motors using this 16-channel board. I am running to some issues about accuracy, for example, when moving a couple of motors do draw a diagonal line, because of the delay between each servo, each motor will move in different timing resulting in incorrect drawings.
I am not sure about how to drive the motors in the fastest way in therms of code.
Where to set delays, the baud rate settings for this application, etc. I couldn't find a good example using all channels with minimum delay. In my case, messages are coming from serial, as explained in the code comment.
Is this the right way to drive this board channels?
I am using an arduino uno, but I would like to check if using a Teensy 3.2 results in best performances for this application.
Thanks in advance for any suggestions.
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
//#define SERVOMIN 150
//#define SERVOMAX 600
// temporary setting pins for 4 lights - it will be controlled by some decade counter...
//#define L1 4
//#define L2 7
//#define L3 8
//#define L4 10
#define L1 9
#define L2 10
#define L3 11
#define L4 12
/*
* a "pointer" device includes a light and 2 servos. Parameters from serial are:
* index,light,servo1,servo2; <- parameters separated by ',' end of pointer is ';'
*
* example of how serial is coming containing instructions for 4 pointers;
0,0,180,180;1,0,0,0;2,0,180,180;3,0,0,0;
0,0,90,90;1,0,90,90;2,0,90,90;3,0,90,90;
**most of the time these instructions doesn't come all for 4 pointers.
ex:
1,0,12,12;4,255,100,100;
**sometimes it comes only id and light parameter.
0,255;1,0;
(instructions only to turn light on/off)
*/
//values for 8 servos:
const uint8_t SERVOMIN[] = {150, 130, 150, 130, 150, 130, 150, 130};
const uint8_t SERVOMAX[] = {600, 500, 600, 500, 600, 500, 600, 500};
//boards (for now, only one board = 16 servos)
Adafruit_PWMServoDriver pwm [] = {
Adafruit_PWMServoDriver(0x40)
};
uint8_t servonum = 0;
uint8_t activeServos = 4; //not being used now
char buf [4]; //maybe too long
uint16_t currentPointer [4]; //index//light//servo1//servo2
byte lightPin [4] = {L1, L2, L3, L4};
uint8_t lightstatus [4] = {0, 0, 0, 0};
//debug
String inputString = ""; // a string to hold incoming data
boolean stringComplete = false; // whether the string is complete
boolean feedback = false;
void setup() {
//temporally as digital outputs
pinMode(L1, OUTPUT);
pinMode(L2, OUTPUT);
pinMode(L3, OUTPUT);
pinMode(L4, OUTPUT);
Serial.begin(115200);//230400 //115200 //57600 //38400 ?
for ( uint8_t i = 0; i < sizeof(pwm); i++) {
pwm[i].begin();
pwm[i].setPWMFreq(60);
}
}
void loop() {
reply();
}
void reply() {
if (stringComplete) {
if (feedback) Serial.println(inputString);
// clear the string:
inputString = "";
stringComplete = false;
for ( int i = 0; i < sizeof(buf); ++i ) buf[i] = (char)0;
}
}
void serialEvent() {
static byte ndx = 0;
static int s = 0;
while (Serial.available()) {
char rc = (char)Serial.read();
inputString += rc;
//(2) setting pointer parameter
if ( rc == ',') {
setPointer(s);
s++;
for ( int i = 0; i < sizeof(buf); ++i ) buf[i] = (char)0;
ndx = 0;
}
//(3) end of this pointer instruction
else if (rc == ';') {
setPointer(s);
//executePointer(); //plan B
ndx = 0;
s = 0;
for ( int i = 0; i < sizeof(buf); ++i ) buf[i] = (char)0;
}
//(4) end of command line
else if (rc == '\n') {
//p = 0;
s = 0;
stringComplete = true;
}
//(1) buffering
else {
buf[ndx] = rc;
ndx++;
}
}
}
void setPointer(int s) {
//index//light//servo1//servo2
int value;
value = atoi(buf);
//index
if (s == 0) {
if (feedback) {
Serial.print("index:");
Serial.print(value);
Serial.print(", buf:");
Serial.println(buf);
}
currentPointer[0] = value;
}
//light
else if (s == 1) {
int index = currentPointer[0];
currentPointer[s] = value;
//Serial.println(index);
digitalWrite(lightPin[index], (value > 0) ? HIGH : LOW);
// analogWrite( lightPin[currentPointer[0]], currentPointer[1]); // implement later
if (feedback) {
Serial.print("light: ");
Serial.println(value);
}
//servos
} else {
int index = currentPointer[0];
if (feedback) {
Serial.print("servo ");
Serial.print(index * 2 + s - 2);
Serial.print(": ");
Serial.println(value);
}
uint16_t pulselen = map(value, 0, 180, SERVOMIN[index], SERVOMAX[index]);
currentPointer[s] = pulselen;
pwm[0].setPWM(index * 2 + (s - 2), 0, pulselen); //current pointer id * 2 + s (s is 2 or 3)
//delay(20);
}
}
// this was plan B - not using
void executePointer() {
int index = currentPointer[0];
analogWrite( lightPin[index], currentPointer[1]);
pwm[0].setPWM(index * 2, 0, currentPointer[2]);
pwm[0].setPWM(index * 2 + 1, 0, currentPointer[3]);
delay(20);
}
I have a color tracking program in Processing, which works with a Kinect. When I click somewhere in the picture it saves this color and draws an ellipse around it. I just want to send 3 int values (one for red, green and blue) over myPort.write() to Arduino and save these 3 values in Arduino in 2 variables. My goal is to light a red LED if the red variable is the highest, and the green LED if green is the highest and so on.
I've tried several examples I found whiel googling, but nothing works. I don't know how Arduino should get the correct values in the variables!
EDIT: Here you have my Processing code. I glued it together from several other tutorials until I nearly cried..
import processing.serial.*;
Serial myPort;
import SimpleOpenNI.*;
SimpleOpenNI kinect;
// Frame
PImage currentFrame;
color trackColor;
int r1, g1, b1, r2, g2, b2;
void setup()
{
size(640, 480);
String portName = Serial.list()[0]; //change the 0 to a 1 or 2 etc. to match your port
myPort = new Serial(this, portName, 9600);
kinect = new SimpleOpenNI(this);
kinect.enableRGB();
trackColor = color (255, 0, 0);
smooth ();
currentFrame = createImage (640, 480, RGB);
}
void draw()
{
kinect.update();
currentFrame = kinect.rgbImage ();
image(currentFrame, 0, 0);
currentFrame.loadPixels();
// Before we begin searching, the "world record" for closest color is set to a high number that is easy for the first pixel to beat.
float worldRecord = 500;
// XY coordinate of closest color
int closestX = 0;
int closestY = 0;
// Begin loop to walk through every pixel
for (int x = 0; x < currentFrame.width; x ++ ) {
for (int y = 0; y < currentFrame.height; y ++ ) {
int loc = x + y*currentFrame.width;
// What is current color
color currentColor = currentFrame.pixels[loc];
r1 = (int)red(currentColor);
g1 = (int)green(currentColor);
b1 = (int)blue(currentColor);
r2 = (int)red(trackColor);
g2 = (int)green(trackColor);
b2 = (int)blue(trackColor);
// Using euclidean distance to compare colors
float d = dist(r1, g1, b1, r2, g2, b2); // We are using the dist( ) function to compare the current color with the color we are tracking.
// If current color is more similar to tracked color than
// closest color, save current location and current difference
if (d < worldRecord) {
worldRecord = d;
closestX = x;
closestY = y;
}
}
}
// We only consider the color found if its color distance is less than 10.
// This threshold of 10 is arbitrary and you can adjust this number depending on how accurate you require the tracking to be.
if (worldRecord < 10) {
// Draw a circle at the tracked pixel
fill(trackColor);
strokeWeight(4.0);
stroke(0);
ellipse(closestX, closestY, 30, 30);
}
if (mousePressed == true) {
color c = get(mouseX, mouseY);
//println("r: " + red(c) + " g: " + green(c) + " b: " + blue(c));
// Save color where the mouse is clicked in trackColor variable
int loc = mouseX + mouseY*(currentFrame.width);
trackColor = currentFrame.pixels[loc];
println("red " + r2);
println("green " + g2);
println("blue " + b2);
int colors[] = {r2, g2, b2};
for(int i=0; i < 3; i++) {
myPort.write(colors[i]);
}
}
println("ClosestX " + closestX);
myPort.write(closestX);
}
And my Arduino Code, where I don't know how to get several values.
int val;
int ledPin = 13;
int freq;
int piezoPin = 9;
int redLED = 3;
int greenLED = 5;
int blueLED = 7;
int red, green, blue;
void setup() {
pinMode(ledPin, OUTPUT); // Set pin as OUTPUT
Serial.begin(9600); // Start serial communication at 9600 bps
digitalWrite(ledPin, LOW);
}
void loop() {
if (Serial.available() > 0)
{ // If data is available to read,
val = Serial.read(); // read it and store it in val
}
if(red > green && red > blue) {
digitalWrite(redLED, HIGH); //light Red LED
}
if(green > red && green > blue) {
digitalWrite(greenLED, HIGH); //light Red LED
}
if(blue > red && blue > green) {
digitalWrite(blueLED, HIGH); //light Red LED
}
//Piezo buzzing higher when X-Position of tracked color is higher.
if (val < 100) {
freq = 50;
}
else if (val < 200) {
freq = 200;
}
else if (val < 300) {
freq = 400;
}
else if (val < 400) {
freq = 600;
}
else if (val < 500) {
freq = 800;
}
else (freq = 1000);
tone(piezoPin, freq);
}
EDIT2: Yes, additionally to lighing the LEDs I also want to have a sound from a piezo buzzer, but that works pretty well, so no questions on that... yet.
Help, please!!
Serial communication to your arduino works with a single byte at a time.
As luck would have it, the three components of a Processing Color are also three bytes.
One for red(0-255)
One for green(0-255)
One for blue(0-255)
Now all we need is a little more info so we can keep them separate.
Because a byte's minimum and maximum values are 0-255, there's no safe character we can use to keep track of the three different bytes, so we need a way to figure out where the info we send begins and ends.
An easy way to do this, is to set up a header and a footer for your messages ; something like :
<color>[byte (red)][byte (green)][byte (blue)]</color>
If we are going to read and decipher messages formatted like this, we are going to need a little buffer that will store the values we receive from Processing, so we can read them back and see if we can match the message format.
So, on the Arduino side, we need this :
String buffer = "";
String messageBegin = "<color>";
String messageEnd = "</color>";
//we read our serial data in the SerialEvent() function
//this is called *after* a loop(), and only if there is serial data in the buffer.
void serialEvent()
{
while(Serial.available())
{
buffer += (char)Serial.read();
}
}
void loop()
{
//now, inside loop, we no longer need to worry about gathering data from serial.
//we do still need to figure out if our message is complete, and then parse it.
//if our buffer contains both the beginning and the end of a message
//in the right order.
int beginIndex = buffer.lastIndexOf(messageBegin);
int endIndex = buffer.lastIndexOf(messageEnd);
if(beginIndex != -1 && endIndex != -1 && beginIndex < endIndex)
{
//we have a complete message!
//our red color starts 7 characters after where the message begins,
//because our "messageBegin" is 7 characters long
string lastMessage = buffer.substring(beginIndex+7);
//this is arguably not the prettiest way to get our byte values back.
//see if you can do better for bonus points!
byte messageAsBytes[80];
lastMessage.getBytes(messageAsBytes, messageAsBytes.length());
//we can now finally reconstruct the value we had from processing!
byte r = (byte)messageAsBytes[0];
byte g = (byte)messageAsBytes[1];
byte b = (byte)messageAsBytes[2];
//if we get a complete message, we can clear our buffer. (don't forget to do this!)
buffer = "";
}
}
On the processing side, all we need to do is make sure our messagebegin and messageend are sent along for the ride :
myPort.write("<color">);
for(int i=0; i < 3; i++) {
myPort.write(colors[i]);
}
myPort.write("</color">);
When I run a piece of code that only gathers values from the temp sensor it returns the correct values.
when I integrate this functionality into a larger piece of code it constantly returns -127 degrees.
#include <OneWire.h>
#include <DallasTemperature.h>
#include <SPP.h>
#include <LiquidCrystal.h>
#define ONE_WIRE_BUS 9
OneWire oneWire(ONE_WIRE_BUS);
DallasTemperature TempSensor(&oneWire);
USB Usb; // create usb
BTD Btd(&Usb); // create bluetooth dongle instance
SPP SerialBT(&Btd, "Arduino", "0000"); // set device name and pin
LiquidCrystal lcd(7, 6, 5, 4, 3, 2); // create lcd instance using declared pins
int LightPin = A0; // pin connected to light sensor
int LcdType = 0; // variable for lcd display
int state; // variable for storing incoming data
int LDRvalue = 0; // value from light sensor
int totalLdr = 0;
int totalLdrNums = 0;
String output = ""; // output string for light sensor
String output2 = ""; // output string for temp sensor
int tempvalue = 0; // value from temp sensor
int totalTemp = 0;
int totalTempNums = 0;
float celsius = 0; // temp in celsius
void setup() {
Serial.begin(9600); // Begin serial comms at speed 9600
if (Usb.Init() == -1) { // if usb hasn't been initilsed
while(1); //wait
}
lcd.begin(16, 2); // set up lcd with 16 coloums and 2 rows
pinMode(LightPin, INPUT); // declare light pin as input
printStart(); // run printStart method
LcdType = 1; // set lcd display type to 1
TempSensor.begin();
}
void loop() {
Usb.Task(); // polls connected devices for status
if(SerialBT.connected) { // if BT connected
if(SerialBT.available() > 0){ // and BT is available
state = SerialBT.read() - '0'; // read incoming value and turn into regular int (0 - 9 in ascii is 48 - 57)
if(state == 0){ // if value read is zero
checkLightValue(); // run checkLightValue method
SerialBT.write(output[0]); // send first value in string
delay(50); // delay before sending next value
SerialBT.write(output[1]); // send second value
delay(50); // delay before sending next value
SerialBT.write(output[2]); // send third value
}
if(state == 1){ // if value read is 1
checkTempValue();
SerialBT.write(output2[0]); // send first value
delay(50); // delay before sending next value
SerialBT.write(output2[1]); // send second value
delay(50); // delay before sending next value
SerialBT.write(output2[3]); // send third value
delay(50); // delay before sending next value
SerialBT.write(output2[5]); // send fourth value
delay(50); // delay before sending next value
}
if(state == 2){
//Restart();
SerialBT.disconnect();
}
if(state == 3){
LcdType = 1;
}
if(state == 4){
LcdType = 2;
}
if(state == 5){
LcdType = 3;
}
if(LcdType == 1){ // if lcd type variable = 1
printLcd();} // run printLcd method
if (LcdType == 2){
printLcd2();
}
if(LcdType == 3){
printLcd3();
}
}
}
}
//void (*resetFunc)(void) = 0;
void checkLightValue(){
output = ""; // reset output string to nothing
LDRvalue = analogRead(LightPin); // read in light value
totalLdr = totalLdr + LDRvalue;
totalLdrNums = totalLdrNums + 1;
int LDR1 = LDRvalue / 100; // get int 1 from value
int twod = LDRvalue - (LDR1 * 100); // get int 2 and 3 from value
int LDR2 = twod / 10; // get int 2 from value
int LDR3 = twod - (LDR2 * 10); // get int 3 from value
output += LDR1; //
output += LDR2; //
output += LDR3; // appends values to string
}
void checkTempValue(){
output2 = ""; // reset output string to nothing
TempSensor.requestTemperatures();
Serial.println(TempSensor.getTempCByIndex(0));
celsius = TempSensor.getTempCByIndex(0);
totalTemp = totalTemp + celsius;
totalTempNums = totalTempNums + 1;
int c1 = celsius / 100; // get first int from value
int twod = celsius - (c1 * 100); //
int c2 = twod / 10;
int c3 = twod - (c2 *10);
output2 += c1; //
output2 += c2; //
output2 += c3; // add values to string to send
}
void printLcd(){
lcd.clear(); // clear lcd
lcd.print("LDR : "); lcd.print(LDRvalue); // display current ldr value
lcd.setCursor(0,1); // set cursor to second row
lcd.print("Temp : "); lcd.print(celsius); // display current temp value on row 2
}
void printLcd2(){
int Averagevalue = totalLdr / totalLdrNums;
lcd.clear();
lcd.print("Average light :");
lcd.setCursor(0,1);
lcd.print(Averagevalue);
}
void printLcd3(){
int Averagevalue = totalTemp / totalTempNums;
lcd.clear();
lcd.print("Average temp :");
lcd.setCursor(0,1);
lcd.print(Averagevalue);
}
void printStart(){
lcd.clear(); // clear lcd
lcd.setCursor(1,1); // set cursor on second row
lcd.print("Waiting for a Connection ?"); // print message
for(int x=1; x<16; x++) { // do following 15 times
lcd.setCursor(x,0); // set cursor to first row
lcd.print("Arduino Started"); // print message // doing this as lcd scrolls means the top line (message) appears static while the bottom line (message) scrolls
lcd.scrollDisplayLeft(); // scroll display left
delay(250); // delay before moving again
}
}
I'm having some trouble with my arduino code. My code has 3 potentiometers that alter each individual colors in a LED, and a button that will fade through red, green, blue (when pressed, it cycles once and when held it continuously cycles). It used to work, but when I tried to add a LCD screen that displays each RGB value, it stopped working. I know it's not written the best, it's just an experimental code as I just got an arduino and am just trying to familiarize myself. When I plug it in, the LCD prints all white squares on the top row and the led is red with flashing blue, and nothing happens when I press the button or turn the pots. Here is the code, any help is appreciated:
#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
int potPin1 = 14 ; // select the input pin for the potentiometer, ANALOG
int potPin2 = 15;
int potPin3 = 16;
int potVal1 = 0; // variable to store the value coming from the sensor
int potVal2 = 0;
int potVal3 = 0;
int ledPin1 = 6; // select the pin for the LED, PWM for analogWrite capability?
int ledPin2 = 9;
int ledPin3 = 10;
int buttonPin = 7;
int buttonState= LOW;
String printCycleVal1 = (""); //print values when cycling colors
String printCycleVal2 = ("");
String printCycleVal3 = ("");
String printVal1 = ("");//print values when not cycling colors
String printVal2 = ("");
String printVal3 = ("");
int val1 = 0; //values to store rgb colors in
int val2 = 0;
int val3 = 0;
void setup()
{
pinMode(potPin1, INPUT);
pinMode(potPin2, INPUT);
pinMode(potPin3, INPUT);
pinMode(ledPin1, OUTPUT);
pinMode(ledPin2, OUTPUT);
pinMode(ledPin2, OUTPUT);
pinMode(buttonPin, INPUT);
lcd.begin(2,16);
}
void loop()
{
lcd.clear();
lcd.setCursor(0,0);
lcd.print("RGB Values: ");
lcd.setCursor(0,1);
potVal1 = analogRead(potPin1);
val1 = map(potVal1, 0, 1023, 0, 255);
analogWrite(ledPin1, val1);
printVal1 += ("V1: ");
printVal1 += (val1);
printVal1 += (" | ");
potVal2 = analogRead(potPin2);
val2 = map(potVal2, 0, 1023, 0, 255);
analogWrite(ledPin2, val2);
printVal2 += ("V2: ");
printVal2 += (val2);
printVal2 += (" | ");
potVal3 = analogRead(potPin3);
val3 = map(potVal3, 0, 1023, 0, 255);
analogWrite(ledPin3, val3);
printVal3 = ("V3: ");
printVal3 += (val3);
buttonState = digitalRead(buttonPin);
while (buttonState == HIGH)
cycle();
}
void cycle() {
setColourRgb(0, 0, 0);
unsigned int rgbColour[3];
// Start off with red.
rgbColour[0] = 255;
rgbColour[1] = 0;
rgbColour[2] = 0;
// Choose the colours to increment and decrement.
for (int decColour = 0; decColour < 3; decColour += 1) {
int incColour = decColour == 2 ? 0 : decColour + 1;
// cross-fade the two colours.
for(int i = 0; i < 255; i += 1) {
rgbColour[decColour] -= 1;
rgbColour[incColour] += 1;
setColourRgb(rgbColour[0], rgbColour[1], rgbColour[2]);
lcd.clear();
lcd.setCursor(0,0);
lcd.print("RGB Values: ");
lcd.setCursor(0,1);
printCycleVal1 += ("V1: ");
printCycleVal1 += (rgbColour[0]);
printCycleVal1 += (" | ");
printCycleVal2 += ("V2: ");
printCycleVal2 += (rgbColour[1]);
printCycleVal2 += (" | ");
printCycleVal3 += ("V3: ");
printCycleVal3 += (rgbColour[2]);
delay(5);
}
buttonState = LOW; n
}
}
void setColourRgb(unsigned int red, unsigned int green, unsigned int blue) {
analogWrite(ledPin1, red);
analogWrite(ledPin2, green);
analogWrite(ledPin3, blue);
}
#include <QTRSensors.h>
#define NUM_SENSORS 5 //Number of sensors used
#define NUM_SAMPLES_PER_SENSOR 4 //Average 4 analog samples per sensor reading
#define EMITTER_PIN 13 //Emitter is controlled by digital pin 2
QTRSensorsAnalog qtra((unsigned char[]) {0,1,2,3,4},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS]; //Variable Array for sensor values
const double kp = .505; //Variable for adjusting KP Value
const double kd = 150; //Variable for adjusting KD Value
const int max_speed = 255; //Variable for the Maximum Speed of the Motors
unsigned int position = 0; //Variable for holding the calculated position
int set_point = 1500;//Variable for holding the value of the center of the line
int error = 0; //Variable for holding the error from center of line based on position
int last_error = 0; //Variable for holding the previous error
int white = 0; //Variable for reading the line over white or black
int max_difference = 80 ;
double spd_right; //Speed for the Right Motor
double spd_left; //Speed for the Left Motor
int derivative = 0; //Value for the derivative
int error_value = 0; //Value for the error_value calculated from the pd function
int key_s6 = 2;//Declaring Digital Push Button
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int M1 = 4; //M1 Direction Control
int M2 = 7; //M2 Direction Control
int sensors_sum = 0;
//Setup Method that includes calibration
void setup(){
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
pinMode(13, OUTPUT);
calibration();
set_Point();
}//Ends Setup
void loop (){
get_sum();
white_or_black();
pid_calc();
adjust_control();
set_motors();
}//Ends Loop*/
/*Method for calibrating set_point on individual tracks
*/
void set_Point(){
qtra.readCalibrated(sensorValues);
for (int i = 0; i <= 5; i++){
set_point = sensorValues[i] + set_point;
}//Ends For
}//Ends set_point Method
/*Method for Calibrating the sensors to the ambiance lighting and
* varying contrasts of each individual track.
*/
void calibration (){
for (int i = 0; i<250; i++){
qtra.calibrate();
delay(20);
}//Ends For
}//Ends Calibration Method
/*Method for calculating the sum of all the sensors to be used in the
* white_or_black method for determining the background color of the track
*/
void get_sum(){
qtra.readCalibrated(sensorValues);
for (int i = 0; i <= 5; i++){
sensors_sum = sensorValues[i] + sensors_sum;
}//Ends For
}//Ends get_sum Method
/*Method for determining the background color of the current portion of the
* track based off of the value returned by get_sum and adjusting a variable
* to follow a white of black line as necessary.
*/
void white_or_black(){
if (sensors_sum > 2000){
white = 1;
}//Ends If
else{
white = 0;
}//Ends Else
}//Ends white_or_black Method
/*Method for following the line based of the position, proportional,
* and derivative
*/
void pid_calc(){
position = qtra.readLine(sensorValues, QTR_EMITTERS_ON, white);
error = position - set_point;
derivative = error-last_error;
last_error = error;
error_value = int(error*kp+derivative*kd);
}//Ends pid_calc Method
void adjust_control(){
if (error_value>max_difference){
error_value = max_difference;
}//Ends If
else if(error_value < max_difference){
error_value = -max_difference;
}//Ends If
if (error_value < 0 ){
spd_right = max_speed + error_value;
spd_left = max_speed;
}//Ends If
else{
spd_right = max_speed;
spd_left = max_speed - error_value;
}//Ends Else
}//Ends adjust_control Method
void set_motors(){
analogWrite(M1, HIGH);
analogWrite(M2, HIGH);
analogWrite(E1, constrain(spd_right, 0, max_speed));
analogWrite(E2, constrain(spd_left, 0, max_speed));
}//Ends set_motors Method
void stop(){
analogWrite(E1, 0);
analogWrite(E2, 0);
}//Ends Stop Method
I'm new to the arduino language. This is my first program and I wrote it last night. It's a PD Line Following example.
I'm currently having issues with the motors adjusting. Simply put it just goes straight.
I'm using a QTR Analog Sensor Array. I might switch to digital because one of the analog inputs on the romeo board I have is not working. Does anyone one have any suggestions or improvements to my logic?
void set_Point(){
qtra.readCalibrated(sensorValues);
for (int i = 0; i < NUM_SENSORS; i++){
set_point += sensorValues[i];
}
}
A strange thing about this loop is that is set_point is not initialised (to zero) before adding, and not divided (by i==NUM_SENSORS-=5) after the summation.