Arduino Uno low memory available - arduino

i'm trying to do a communication over ethernet using ENC28J60 and Arduino Uno and run 3 task . I have a little problem with my arduino code. My code is compiling but i get the following error : "Low memory available, stability problems may occur." and the led on the board is blinking very fast. I guess that the board is trying to alocate memory but he faild. Any idea what can i do ?
#include <Arduino_FreeRTOS.h>
#include "HX711.h"
#include <PID_v1.h>
#include <string.h>
//#include <SPI.h>
#include <UIPEthernet.h>
#define configUSE_IDLE_HOOK 0
// FreeRTOS tasks
void TaskPrimaryControlLoop(void *pvParameters);
void TaskConcentrationControlLoop(void *pvParameters);
void TaskIdle(void *pvParameters);
/* Weigth Cells */
#define hx_pf_dout 3
#define hx_pf_clk 2
#define hx_c_dout 5
#define hx_c_clk 4
HX711 pf_scale(hx_pf_dout, hx_pf_clk);
HX711 c_scale(hx_c_dout, hx_c_clk);
float pf_factor = -236000;
float c_factor = -203000;
float p_weigth = 0; // (kg, 0.000) primary liquid weigth
float p_l_weigth = 0; // (kg, 0.000) primary liquid last weigth
float c_weigth = 0; // (kg, 0.000) concentrate liquid weigth
float c_l_weight = 0; // (kg, 0.000) concentrate liquid last weigth
/* h bridge config */
#define speed_p 9
#define forward_p 7
#define backward_p 8
#define speed_c 6
#define forward_c A0
#define backward_c A1
double p_pump = 0; // 0-255 pwm pump output
double c_pump = 0; // 0-255 pwm pump output
//// PID parameters
// Primary Control Loop
#define p_kp 250.0
#define p_ki 25.0
// Concentration Control Loop
#define c_kp 250.0
#define c_ki 25.0
double p_pv = 0; // (%) primary flow value
double c_pv = 0; // (%) concentration flow value
double p_sp = 0; // (l/min) primary flow setpoint
double c_sp_proc = 0; // % concentration
double c_sp = 0; // (l/min) concentration setpoint
PID pid_pcl(&p_pv, &p_pump, &p_sp, p_kp,p_ki,0.0, DIRECT);
PID pid_ccl(&c_pv, &c_pump, &c_sp, c_kp,c_ki,0.0, DIRECT);
/* Communication Ethernet */
#define MAX_STRING_LEN 32
const byte numChars = 32;
char receivedChars[numChars];
boolean newData = false;
byte mac[] = {
0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
};
IPAddress ip(192, 168, 1, 2);
IPAddress myDns(192,168,1, 1);
IPAddress gateway(192, 168, 1, 1);
IPAddress subnet(255, 255, 255, 0);
// telnet defaults to port 23
EthernetServer server(23);
//EthernetClient client;
boolean alreadyConnected = false; // whether or not the client was connected previously
void setup() {
Serial.begin(9600);
while (!Serial) {
; // wait for serial port to connect. Needed for native USB, on LEONARDO, MICRO, YUN, and other 32u4 based boards.
}
pinMode(forward_p, OUTPUT);
pinMode(backward_p, OUTPUT);
pinMode(forward_c, OUTPUT);
pinMode(backward_c, OUTPUT);
pinMode(speed_p, OUTPUT);
pinMode(speed_c, OUTPUT);
digitalWrite(backward_p, HIGH);
digitalWrite(backward_c, HIGH);
digitalWrite(forward_p, LOW);
digitalWrite(forward_c, LOW);
pf_scale.set_scale(pf_factor);
c_scale.set_scale(c_factor);
//pf_scale.tare();
//c_scale.tare();
pid_ccl.SetMode(AUTOMATIC);
pid_pcl.SetMode(AUTOMATIC);
xTaskCreate(
TaskPrimaryControlLoop
, (const portCHAR *)"PrimaryControlLoop" // A name just for humans
, 128 // This stack size can be checked & adjusted by reading the Stack Highwater
, NULL
, 2 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
, NULL );
xTaskCreate(
TaskConcentrationControlLoop
, (const portCHAR *)"ConcentrationControlLoop" // A name just for humans
, 128 // This stack size can be checked & adjusted by reading the Stack Highwater
, NULL
, 2 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
, NULL );
xTaskCreate(
TaskIdle
, (const portCHAR *)"Idle" // A name just for humans
, 512 // This stack size can be checked & adjusted by reading the Stack Highwater
, NULL
, 0 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
, NULL );
}
void loop() {
}
void TaskPrimaryControlLoop(void *pvParameters)
{
//(void) pvParameters;
for (;;)
{
p_weigth = pf_scale.get_units(1);
p_pv = (p_l_weigth - p_weigth)*100;
if(p_pv < 0) p_pv = 0;
pid_pcl.Compute();
analogWrite(speed_p, p_pump);
p_l_weigth = p_weigth;
vTaskDelay(200 / portTICK_PERIOD_MS); // 200 ms sample time
}
}
void TaskConcentrationControlLoop(void *pvParameters)
{
//(void) pvParameters;
for (;;)
{
c_weigth = c_scale.get_units(1);
c_pv = (c_l_weight - c_weigth)*100;
if(c_pv < 0) c_pv = 0;
c_sp = p_sp * (c_sp_proc/100);
pid_ccl.Compute();
analogWrite(speed_c, c_pump);
c_l_weight = c_weigth;
vTaskDelay(200 / portTICK_PERIOD_MS); // 200 ms sample time
}
}
void TaskIdle(void *pvParameters)
{
//(void) pvParameters;
for(;;){
EthernetClient client = server.available();
// when the client sends the first byte, say hello:
if (client) {
if (!alreadyConnected) {
client.flush();
//Serial.println("We have a new client");
//client.println("Hello, client!");
alreadyConnected = true;
}
}
recvWithStartEndMarkers();
//showNewData();
if(receivedChars[0] == 's'){
p_sp = atof(subStr(receivedChars, ",", 2));
c_sp_proc = int(subStr(receivedChars, ",", 3));
newData = false;
memset(receivedChars, 0, sizeof(receivedChars));
}
// send process values to application
if(receivedChars[0] == 'w'){
Serial.print(p_pv);
Serial.print(",");
Serial.print(p_sp);
Serial.print(",");
Serial.print(int(c_pump));
Serial.print(",");
Serial.print(c_pv);
Serial.print(",");
Serial.print(c_sp);
Serial.print(",");
Serial.print(int(p_pump));
Serial.println();
newData = false;
memset(receivedChars, 0, sizeof(receivedChars));
}
/*
// check commands
while(Serial.available() > 7){
p_sp = Serial.parseFloat();
c_sp_proc = Serial.parseInt();
}
newData = false;
memset(receivedChars, 0, sizeof(receivedChars)
*/
newData = false;
memset(receivedChars, 0, sizeof(receivedChars));
vTaskDelay(1);
}
}
void recvWithStartEndMarkers() {
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = '<';
char endMarker = '>';
char rc;
EthernetClient client = server.available();
while (client.available() > 0 && newData == false) {
rc = client.read();
if (recvInProgress == true) {
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
recvInProgress = false;
ndx = 0;
newData = true;
}
}
else if (rc == startMarker) {
recvInProgress = true;
}
}
}
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
// Function to return a substring defined by a delimiter at an index
char* subStr (char* str, char *delim, int index) {
char *act, *sub, *ptr;
static char copy[MAX_STRING_LEN];
int i;
// Since strtok consumes the first arg, make a copy
strcpy(copy, str);
for (i = 1, act = copy; i <= index; i++, act = NULL) {
//Serial.print(".");
sub = strtok_r(act, delim, &ptr);
if (sub == NULL) break;
}
return sub;
}

First thing to do is try to increase the stack depth of your Tasks.
You're currently using 128, 128 and 512.
You can use "StackHighWaterMark" to get the info about the amount of stack space remaining. Try to use this information to "calibrate" the depth of your task. I recommend using at least 40% of free space.

In order to save space in the Arduino memory you can put all the Serial.print in the flash memory, try to use this syntax:
Serial.print(F(","));
Basically you have to add an F in front of the string that you want to print, but you can't do that when you print a variable:
Serial.print(int(c_pump)); // you can't do that here

Related

MPU6050 FIFO overflow and freezing problems

Im using the sensor MPU6050 to control movements of my robotic arm. The codes work fine when it is a standalone program but i keep encountering 'FIFO overflow' when the codes are complied into the main program. This is the code that i am using.
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
RF24 radio(9, 8); // CE, CSN
const byte address[6] = "00001";
const int AccReadings = 10;
//Wrist Roll
int DataX[AccReadings];
int WRIndex = 0;
int WRtotal = 0;
int WRaverage = 0;
//Wrist Pitch
int DataY[AccReadings];
int WPIndex = 0;
int WPtotal = 0;
int WPaverage = 0;
//Shoulder Lift
int DataY2[AccReadings];
int SLIndex = 0;
int SLtotal = 0;
int SLaverage = 0;
//Elbow Lift
int ELaverage = 0;
//Arm Rotation
int ARaverage = 0;
float correct;
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
struct Sensor_Data
{
int WristRoll;
int WristPitch;
int ShoulderLift;
int ElbowLift;
int ArmRotation;
};
Sensor_Data data;
//Interrupt Detection
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
radio.begin();
radio.openWritingPipe(address);
radio.setPALevel(RF24_PA_MIN);
radio.stopListening();
//Zero-fill Arrays
for (int i = 0; i < AccReadings; i++)
{
DataX[i] = 0;
}
for (int j = 0; j < AccReadings; j++)
{
DataY[j] = 0;
}
for (int k = 0; k < AccReadings; k++)
{
DataY2[k] = 0;
}
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(49);
mpu.setYGyroOffset(-18);
mpu.setZGyroOffset(9);
mpu.setZAccelOffset(4427);
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
// Serial.print(F("DMP Initialization failed (code "));
//Serial.print(devStatus);
//Serial.println(F(")"));
}
}
void loop()
{
/*smoothWR();
movementWR();
smoothWP();
movementWP();
smoothSL();
movementSL();*/
ElbowMovement();
radio.write(&data, sizeof(Sensor_Data));
}
void smoothWR()
{
WRtotal = WRtotal - DataX[WRIndex];
DataX[WRIndex] = analogRead(A0);
WRtotal = WRtotal + DataX[WRIndex];
WRIndex = WRIndex + 1;
if (WRIndex >= AccReadings)
{
WRIndex = 0;
}
WRaverage = WRtotal / AccReadings;
//Serial.println(WRaverage);
}
void movementWR()
{
WRaverage = map(WRaverage, 278, 419, 0, 180);
data.WristRoll = constrain(WRaverage, 0, 180);
//Serial.println(data.WristRoll);
}
void smoothWP()
{
WPtotal = WPtotal - DataY[WPIndex];
DataY[WPIndex] = analogRead(A1);
WPtotal = WPtotal + DataY[WPIndex];
WPIndex = WPIndex + 1;
if (WPIndex >= AccReadings)
{
WPIndex = 0;
}
WPaverage = WPtotal / AccReadings;
//Serial.println(WPaverage);
}
void movementWP()
{
WPaverage = map(WPaverage, 280, 421, 0 , 135);
data.WristPitch = constrain(WPaverage, 0, 135);
//Serial.println(data.WristPitch);
}
void smoothSL()
{
SLtotal = SLtotal - DataY2[SLIndex];
DataY2[SLIndex] = analogRead(A2);
SLtotal = SLtotal + DataY2[SLIndex];
SLIndex = SLIndex + 1;
if (SLIndex >= AccReadings)
{
SLIndex = 0;
}
SLaverage = SLtotal / AccReadings;
//Serial.println(SLaverage);
}
void movementSL()
{
SLaverage = map(SLaverage, 410, 270, 0 , 180);
data.ShoulderLift = constrain(SLaverage, 35, 180);
//Serial.println(data.ShoulderLift);
}
void ElbowMovement()
{
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize)
{
if (mpuInterrupt && fifoCount < packetSize)
{
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT))
{
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// Get Yaw, Pitch and Roll values
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// Yaw, Pitch, Roll values - Radians to degrees
ypr[0] = ypr[0] * 180 / M_PI;
ypr[1] = ypr[1] * 180 / M_PI;
ypr[2] = ypr[2] * 180 / M_PI;
// Skip 300 readings (self-calibration process)
if (int l = 0; l <= 300) {
correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
l++;
}
// After 300 readings
else {
ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract the last random Yaw value from the currrent value to make the Yaw 0 degrees
// Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
ELaverage = map(ypr[0], -90, 90, 0, 180);
data.ElbowLift = constrain(ELaverage, 30, 110);
ARaverage = map(ypr[1], -90, 90, 0, 180);
data.ArmRotation = constrain(ARaverage, 0, 180);
//Serial.println(data.ElbowLift);
Serial.println(ypr[1]);
}
#endif
}
}
Is there any ways to get rid of the FIFO overflow? Also When i tried to used Jeff Rowberg's example codes MPU6050_DMP6 the program will freeze after a few seconds. Is there any solution to that? These are the example codes that i am using.
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
float correct;
int j = 0;
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
//Interrupt Detection
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
void setup()
{
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(38400);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(17);
mpu.setYGyroOffset(-69);
mpu.setZGyroOffset(27);
mpu.setZAccelOffset(1551);
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
// Serial.print(F("DMP Initialization failed (code "));
//Serial.print(devStatus);
//Serial.println(F(")"));
}
}
void loop()
{
if (!dmpReady) return;
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
{
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
if (j <= 300)
{
correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
j++;
}
else
{
ypr[0] = ypr[0] - correct;
Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
}
#endif
}
}
You're using DMP (Digital Motion Processor) with mean calculation running on the MPU itself, this gives more precise and less CPU consumption but you need to update the FIFO frequently or the track goes wrong.
Encountering 'FIFO overflow' means your loop code was too slow, you should increase the speed of another task in your loop code. Or just use other code that not use the DMP.

Arduino I2C Slave to Master communication problem

I am having a problem with reading random data in my Arduino Mega (Master) from my Arduino Uno (Slave) while using I2C communication.
Some background: I am reading Encoder data from the Uno and sending to the Mega via I2C communication. The encoder data is been used in the MEga to adjust the speed of a motor so that the revolutions per second of the different wheels have the same value.
The issue of reading random data arises when I include an IF condition or function.
Even if the IF condition included is an empty one or a call to function which has an empty body it starts to read random wrong data from the Uno.
If i don't have the adjusting part (IF condition/ function) of the code the reading of the data from the Uno works fine.
If anybody can help, it would be greatly appreciated.
Master Code:
#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>
// Include the required Wire library for I2C<br>#include
#include <Wire.h>
// RX on pin 17 (to S2), TX on pin 16 (to S1).
SoftwareSerial SWSerial(NOT_A_PIN, 16);
// Use SWSerial as the serial port.
SabertoothSimplified ST(SWSerial);
//////////////////ENCODER DATA//////////////////
unsigned int revolutions_L_rpm = 0;
unsigned int revolutions_R_rpm = 0;
int16_t x = 0;
int16_t y = 0;
////////////////////////////////////////////////
//////////////VARIABLES FOR ADJUST//////////////
int error = 0;
int kp = 12;
int adjusted = 0;
////////////////////////////////////////////////
////////////////////MOTORS//////////////////////
//Declare the arduino pins
int LEDg = 7;
int LEDr = 6;
int LEDy = 5;
int speedVar = 0;
int speedOne = 0;
int speedTwo = 0;
int power;
////////////////////END/////////////////////////
void setup() {
//initlize the mode of the pins
pinMode(LEDg,OUTPUT);
pinMode(LEDr,OUTPUT);
pinMode(LEDy,OUTPUT);
//set the serial communication rate
Serial.begin(9600);
SWSerial.begin(9600);
Wire.begin();
}
void loop()
{
//check whether arduino is reciving signal or not
if(Serial.available() > 0){
char val = Serial.read();//reads the signal
Serial.print("Recieved: ");
Serial.println(val);
switch(val){
/*********Increase speed by 1 as long as e(triangle) is held*********/
case 'a':
forward();
break;
/*********Decrease speed by 1 as long as g(x) is held*********/
case 'c':
reverse();
break;
/*********Increase speed by 1 as long as e(triangle) is held*********/
case 'd':
turnLeft();
break;
/*********Decrease speed by 1 as long as g(x) is held*********/
case 'b':
turnRight();
break;
/*********Toggle when Circle is held for 5 seconds*********/
case 'f':
toggleSwitch(LEDy);
break;
/*********Toggle when Square is held for 5 seconds*********/
case 'h':
stopMotors();
break;
}
Serial.print("sppedVar = ");
Serial.print(speedVar);
Serial.print("\tleftSpeed: ");
Serial.print(speedOne);
Serial.print("\trightSpeed: ");
Serial.println(speedTwo);
}
Wire.requestFrom(9,4); // Request 4 bytes from slave arduino (9)
byte a = Wire.read();
Serial.print("a: ");
Serial.print(a);
byte b = Wire.read();
Serial.print(" b: ");
Serial.print(b);
byte e = Wire.read();
Serial.print(" --- e: ");
Serial.print(e);
byte f = Wire.read();
Serial.print(" f: ");
Serial.print(f);
x = a;
x = (x << 8) | b;
Serial.print("\tX: ");
Serial.print(x);
y = e;
y = (y << 8) | f;
Serial.print("\tY: ");
Serial.print(y);
revolutions_L_rpm = x;
revolutions_R_rpm = y;
if ((revolutions_L_rpm != revolutions_R_rpm) && (speedVar != 0)){
error = 0;
error = revolutions_L_rpm - revolutions_R_rpm;
adjusted = error/kp;
Serial.print("Error: ");
Serial.print(error);
Serial.print("Error/kp: ");
Serial.println(adjusted);
if ((speedTwo < 20) && (speedTwo > -20)){
speedTwo -= adjusted;
power = speedTwo;
ST.motor(2, -power);
//delay(20);
}
}
// Print out rpm
Serial.print("Left motor rps*100: ");
Serial.print(revolutions_L_rpm);
Serial.print(" ///// Right motor rps*100: ");
Serial.println(revolutions_R_rpm);
// Print out speed
Serial.print("speedOne: ");
Serial.print(speedOne);
Serial.print("\tspeedTwo: ");
Serial.println(speedTwo);
delay(1000);
}
Slave code:
// Include the required Wire library for I2C<br>#include <Wire.h>
#include <Wire.h>
// Checked for main program
volatile boolean counterReady;
// Internal to counting routine
unsigned int timerPeriod;
unsigned int timerTicks;
unsigned long overflowCount;
// The pin the encoder is connected
int encoder_in_L = 2;
int encoder_in_R = 3;
// The number of pulses per revolution
// depends on your index disc!!
unsigned int pulsesperturn = 16;
// The total number of revolutions
int16_t revolutions_L = 0;
int16_t revolutions_R = 0;
int16_t revolutions_L_rpm = 0;
int16_t revolutions_R_rpm = 0;
// Initialize the counter
int16_t pulses_L = 0;
int16_t pulses_R = 0;
byte myData[4];
// This function is called by the interrupt
void count_L() {
pulses_L++;
}
void count_R() {
pulses_R++;
}
void startCounting(unsigned int ms) {
counterReady = false; // time not up yet
timerPeriod = ms; // how many ms to count to
timerTicks = 0; // reset interrupt counter
overflowCount = 0; // no overflows yet
// Reset timer 2
TCCR2A = 0;
TCCR2B = 0;
// Timer 2 - gives us our 1 ms counting interval
// 16 MHz clock (62.5 ns per tick) - prescaled by 128
// counter increments every 8 µs.
// So we count 125 of them, giving exactly 1000 µs (1 ms)
TCCR2A = bit (WGM21) ; // CTC mode
OCR2A = 124; // count up to 125 (zero relative!!!!)
// Timer 2 - interrupt on match (ie. every 1 ms)
TIMSK2 = bit (OCIE2A); // enable Timer2 Interrupt
TCNT2 = 0; // set counter to zero
// Reset prescalers
GTCCR = bit (PSRASY); // reset prescaler now
// start Timer 2
TCCR2B = bit (CS20) | bit (CS22) ; // prescaler of 128
}
ISR (TIMER2_COMPA_vect){
// see if we have reached timing period
if (++timerTicks < timerPeriod)
return;
TCCR2A = 0; // stop timer 2
TCCR2B = 0;
TIMSK2 = 0; // disable Timer2 Interrupt
counterReady = true;
if(counterReady){
Serial.print("Pulses_L: ");
Serial.print(pulses_L);
Serial.print(" Pulses_R: ");
Serial.println(pulses_R);
// multiplying by 100 to get a greater difference to compare
revolutions_L_rpm = (pulses_L * 100) / pulsesperturn;
revolutions_R_rpm = (pulses_R * 100) / pulsesperturn;
// Total revolutions
// revolutions_L = revolutions_L + (pulses_L / pulsesperturn);
// revolutions_R = revolutions_R + (pulses_R / pulsesperturn);
pulses_L = 0;
pulses_R = 0;
}
}
void requestEvent() {
myData[0] = (revolutions_L_rpm >> 8) & 0xFF;
myData[1] = revolutions_L_rpm & 0xFF;
myData[2] = (revolutions_R_rpm >> 8) & 0xFF;
myData[3] = revolutions_R_rpm & 0xFF;
Wire.write(myData, 4); //Sent 4 bytes to master
}
void setup() {
Serial.begin(9600);
pinMode(encoder_in_L, INPUT);
pinMode(encoder_in_R, INPUT);
attachInterrupt(0, count_L, RISING); //attachInterrupt(digitalPinToInterrupt(encoder_in_L, count_L, RISING);
attachInterrupt(1, count_R, RISING); //attachInterrupt(digitalPinToInterrupt(encoder_in_R, count_R, RISING);
// Start the I2C Bus as Slave on address 9
Wire.begin(9);
// Attach a function to trigger when something is received.
Wire.onRequest(requestEvent);
}
void loop() {
// stop Timer 0 interrupts from throwing the count out
byte oldTCCR0A = TCCR0A;
byte oldTCCR0B = TCCR0B;
TCCR0A = 0; // stop timer 0
TCCR0B = 0;
startCounting (1000); // how many ms to count for
while (!counterReady)
{ } // loop until count over
// Print out rpm
Serial.print("Left motor rps: ");
Serial.println(revolutions_L_rpm);
Serial.print("Right motor rps: ");
Serial.println(revolutions_R_rpm);
// Print out revolutions
// Serial.print("Left motor revolution count: ");
// Serial.println(revolutions_L);
// Serial.print("Right motor revolution count: ");
// Serial.println(revolutions_R);
// restart timer 0
TCCR0A = oldTCCR0A;
TCCR0B = oldTCCR0B;
delay(200);
}

Divided and duplicated frame capture output for ADNS 9800 Arduino

I am trying to interface an ADNS 9800 mouse chip which I took from "Sharkoon SHARK ZONE M50" . The original PCB is still in place.
I am trying to obtain a framecapture, which should be 30 x 30 pixels. I have connected the ADNS 9800 with SPI to an Arduino UNO Rev 3 (i.e. 5V operating voltage). I.e. MISO, MOSI, SCLK, DGND, AGND, NCS. I did not connect any voltage, since I concluded from previous attempts that that did not yield a good frame capture.
The current problem is that I get a frame capture that is divided in 3 parts: square top left (with a good image of the surroundings), square bottom left (which is a duplicate of top left) and a rectangle on the right half of the screen of monotone grey colour (which does change depending on light conditions). See image. I want the full screen to be one image, not the divided mess it is now. Therefore, it may be a question of the resolution that is used, it may be that it is 15x15 instead of 30x30. However I do not know where this is determined/set.
Also, I find it strange that no input voltage seems to be needed to obtain an image from the camera.
See attachments for frame capture and code (arduino + processing).
Frame output
Arduino code
#include
#include
// Registers
#define REG_Product_ID 0x00
#define REG_Revision_ID 0x01
#define REG_Motion 0x02
#define REG_Delta_X_L 0x03
#define REG_Delta_X_H 0x04
#define REG_Delta_Y_L 0x05
#define REG_Delta_Y_H 0x06
#define REG_SQUAL 0x07
#define REG_Pixel_Sum 0x08
#define REG_Maximum_Pixel 0x09
#define REG_Minimum_Pixel 0x0a
#define REG_Shutter_Lower 0x0b
#define REG_Shutter_Upper 0x0c
#define REG_Frame_Period_Lower 0x0d
#define REG_Frame_Period_Upper 0x0e
#define REG_Configuration_I 0x0f
#define REG_Configuration_II 0x10
#define REG_Frame_Capture 0x12
#define REG_SROM_Enable 0x13
#define REG_Run_Downshift 0x14
#define REG_Rest1_Rate 0x15
#define REG_Rest1_Downshift 0x16
#define REG_Rest2_Rate 0x17
#define REG_Rest2_Downshift 0x18
#define REG_Rest3_Rate 0x19
#define REG_Frame_Period_Max_Bound_Lower 0x1a
#define REG_Frame_Period_Max_Bound_Upper 0x1b
#define REG_Frame_Period_Min_Bound_Lower 0x1c
#define REG_Frame_Period_Min_Bound_Upper 0x1d
#define REG_Shutter_Max_Bound_Lower 0x1e
#define REG_Shutter_Max_Bound_Upper 0x1f
#define REG_LASER_CTRL0 0x20
#define REG_Observation 0x24
#define REG_Data_Out_Lower 0x25
#define REG_Data_Out_Upper 0x26
#define REG_SROM_ID 0x2a
#define REG_Lift_Detection_Thr 0x2e
#define REG_Configuration_V 0x2f
#define REG_Configuration_IV 0x39
#define REG_Power_Up_Reset 0x3a
#define REG_Shutdown 0x3b
#define REG_Inverse_Product_ID 0x3f
#define REG_Snap_Angle 0x42
#define REG_Motion_Burst 0x50
#define REG_SROM_Load_Burst 0x62
#define REG_Pixel_Burst 0x64
byte initComplete=0;
byte testctr=0;
unsigned long currTime;
unsigned long timer;
volatile int xdat;
volatile int ydat;
volatile byte movementflag=0;
const int ncs = 10;
const int lsPin = 4;//ANALOG
const int linearActPin = 9;
extern const unsigned short firmware_length;
extern const unsigned char firmware_data[];
String parseChar = ".";
void setup() {
Serial.begin(115200);
//For first parse put LF and CR there
Serial.println("");
//pinMode(ls, INPUT);
//ADNS 9800 setup
pinMode (ncs, OUTPUT);
SPI.begin();
SPI.setDataMode(SPI_MODE3);
SPI.setBitOrder(MSBFIRST);
//Set clock to 2 MHz
SPI.setClockDivider(8);
performStartup();
dispRegisters();
delay(100);
//Pin modes
pinMode(linearActPin, OUTPUT);
Serial.print("Ready");
Serial.println(parseChar);
//Serial.println("Device is ready");
//FrameCapture();
}
/* DO NOT EDIT BELOW; NECESSARY FOR ADNS9800 */
void performStartup(void){
// reset the chip
adns_com_end(); // ensure that the serial port is reset
adns_com_begin(); // ensure that the serial port is reset
adns_com_end(); // ensure that the serial port is reset
adns_write_reg(REG_Power_Up_Reset, 0x5a); // force reset
delay(50); // wait for it to reboot
// read registers 0x02 to 0x06 (and discard the data)
adns_read_reg(REG_Delta_X_L);
adns_read_reg(REG_Delta_X_H);
adns_read_reg(REG_Delta_Y_L);
adns_read_reg(REG_Delta_Y_H);
// upload the firmware
adns_upload_firmware();
delay(10);
//enable laser(bit 0 = 0b), in normal mode (bits 3,2,1 = 000b)
// reading the actual value of the register is important because the real
// default value is different from what is said in the datasheet, and if you
// change the reserved bytes (like by writing 0x00...) it would not work.
byte laser_ctrl0 = adns_read_reg(REG_LASER_CTRL0);
adns_write_reg(REG_LASER_CTRL0, laser_ctrl0 & 0xf1 );
//0x08 = enable fixed framerate, leave rest standard
//0x10 = disable AGC, leave rest standard
adns_write_reg(REG_Configuration_II, 0x08);
//Set resolution; cpi = REG_value x50
//Min: 0x01 50 cpi
//Max: 0xA4 8200 cpi
adns_write_reg(REG_Configuration_I, 0xA4);
//Set fixed framerate: FR = clk_freq/REG_value = 2000 fps
adns_write_reg(REG_Frame_Period_Max_Bound_Lower, 0xa8);
adns_write_reg(REG_Frame_Period_Max_Bound_Upper, 0x61);
//Set shutter time
adns_write_reg(REG_Shutter_Max_Bound_Lower,0x00);
adns_write_reg(REG_Shutter_Max_Bound_Upper,0x08);
//adns_write_reg(REG_Snap_Angle, 0x80);
delay(1);
Serial.print("Initialized");
Serial.println(parseChar);
}
void adns_com_begin(){
digitalWrite(ncs, LOW);
}
void adns_com_end(){
digitalWrite(ncs, HIGH);
}
byte adns_read_reg(byte reg_addr){
adns_com_begin();
// send adress of the register, with MSBit = 0 to indicate it's a read
SPI.transfer(reg_addr & 0x7f );
delayMicroseconds(100); // tSRAD
// read data
byte data = SPI.transfer(0);
delayMicroseconds(1); // tSCLK-NCS for read operation is 120ns
adns_com_end();
delayMicroseconds(19); // tSRW/tSRR (=20us) minus tSCLK-NCS
return data;
}
void adns_write_reg(byte reg_addr, byte data){
adns_com_begin();
//send adress of the register, with MSBit = 1 to indicate it's a write
SPI.transfer(reg_addr | 0x80 );
//sent data
SPI.transfer(data);
delayMicroseconds(20); // tSCLK-NCS for write operation
adns_com_end();
delayMicroseconds(100); // tSWW/tSWR (=120us) minus tSCLK-NCS. Could be shortened, but is looks like a safe lower bound
}
void adns_upload_firmware(){
// send the firmware to the chip, cf p.18 of the datasheet
//Serial.println("Uploading firmware...");
// set the configuration_IV register in 3k firmware mode
adns_write_reg(REG_Configuration_IV, 0x02); // bit 1 = 1 for 3k mode, other bits are reserved
// write 0x1d in SROM_enable reg for initializing
delay(10);
adns_write_reg(REG_SROM_Enable, 0x1d);
// wait for more than one frame period
delay(10); // assume that the frame rate is as low as 100fps... even if it should never be that low
// write 0x18 to SROM_enable to start SROM download
adns_write_reg(REG_SROM_Enable, 0x18);
// write the SROM file (=firmware data)
adns_com_begin();
//write burst destination adress
//bitwise OR to ensure MSB is 1
SPI.transfer(REG_SROM_Load_Burst | 0x80);
delayMicroseconds(50);
// send all bytes of the firmware
unsigned char c;
for(int i = 0; i < firmware_length; i++){
c = (unsigned char)pgm_read_byte(firmware_data + i);
SPI.transfer(c);
delayMicroseconds(15);
}
adns_com_end();
}
void adns_frame_capture(){
//Send signal to start datacollection frame capture
Serial.print("Frame capture");
Serial.println(parseChar);
// reset the chip
adns_write_reg(REG_Power_Up_Reset, 0x5a); // force reset
delay(50); // wait for it to reboot
delay(10);
//Write bytes to Frame_Capture
adns_write_reg(REG_Frame_Capture, 0x93);
adns_write_reg(REG_Frame_Capture, 0xc5);
// wait for more than two frame periods
delay(25); // assume that the frame rate is as low as 100fps... even if it should never be that low
//Check for the first pixel bij reading bit zero of Motion register
//If it is 1, first pixel available
byte motion = adns_read_reg(REG_Motion);
adns_com_begin();
delayMicroseconds(120);//delay t-SRAD = 100 us
byte pixel_burst;
if (motion == 0x21){
//Reading pixel values from ADNS and storing them in Array
for(int i = 0; i < 900; i++){
pixel_burst = adns_read_reg(REG_Pixel_Burst);
//Serial.print(i);
//Serial.print(":");
Serial.print(String(pixel_burst));
Serial.println(parseChar);
delayMicroseconds(15);
}
//Finished transmitting data
Serial.print("Data transfer finished");
Serial.println(parseChar);
//Transfer surface quality value
Serial.print("SQUAL");
Serial.print(String(adns_read_reg(REG_SQUAL)));
Serial.println(parseChar);
}else {
Serial.print("Frame capture failed");
Serial.println(parseChar);
}
adns_com_end();
//Hardware reset and firmware restore required to return navigation
performStartup();
}
void dispRegisters(void){
int oreg[7] = {
0x00,0x3F,0x2A,0x02 };
char* oregname[] = {
"Product_ID","Inverse_Product_ID","SROM_Version","Motion" };
byte regres;
digitalWrite(ncs,LOW);
int rctr=0;
for(rctr=0; rctr<4; rctr++){
SPI.transfer(oreg[rctr]);
delay(1);
//Serial.println("---");
//Serial.println(oregname[rctr]);
//Serial.println(oreg[rctr],HEX);
regres = SPI.transfer(0);
//Serial.println(regres,BIN);
//Serial.println(regres,HEX);
delay(1);
}
digitalWrite(ncs,HIGH);
}
/*********************************************************
DO NOT EDIT ABOVE; NECESSARY FOR RUNNING ADNS9800
*********************************************************/
String data = String();
//Process variables
int run = 0;
int t = 0;
unsigned long t_ms, t_us;
int dt = 0;//1/f = [ms]
long int t_run = 0;//[ms]
unsigned long ms_start, us_start;
void loop() {
if (dt == -1 || t_run == -1){
Serial.print("Time constant error");
Serial.println(parseChar);
}else if (run == 1 && t<t_run){
measure();
Serial.print(data);
Serial.println("");
Serial.println(parseChar);
}else if(run == 1 && t>=t_run){
//Measurement finished
Serial.print("Measurement finished");
Serial.println(parseChar);
digitalWrite(linearActPin, LOW);
run = 0;
t = 0;
}
}
void serialEvent(){
String data_rx;
if (Serial.available() > 0){
//Parse serial data until '.'
data_rx = Serial.readStringUntil('.');
//Remove '.' from buffer
data_rx = data_rx.substring(0, data_rx.length());
//Serial.print(data_rx);
if (data_rx.equals("Run")){
run = 1;
ms_start = millis();
us_start = micros();
digitalWrite(linearActPin, HIGH);
//Read registers and discard data
byte XDataL = adns_read_reg(REG_Delta_X_L);
byte XDataH = adns_read_reg(REG_Delta_X_H);
byte YDataL = adns_read_reg(REG_Delta_Y_L);
byte YDataH = adns_read_reg(REG_Delta_Y_H);
}else if(data_rx.equals("Frame capture run")){
adns_frame_capture();
}else if(data_rx.equals("SQUAL")){
Serial.println(String(adns_read_reg(REG_SQUAL)));
}else if(data_rx.startsWith("dt")){
dt = data_rx.substring(2,data_rx.length()).toInt();
}else if(data_rx.startsWith("trun")){
t_run = data_rx.substring(4,data_rx.length()).toInt();
}
}
}
void measure(void){
/*READ dx, dy, ls
increment t with dt
return String "t,dx,dy,ls"*/
//Read optic flow from ADNS
byte XDataL = adns_read_reg(REG_Delta_X_L);
byte XDataH = adns_read_reg(REG_Delta_X_H);
byte YDataL = adns_read_reg(REG_Delta_Y_L);
byte YDataH = adns_read_reg(REG_Delta_Y_H);
int ls;
unsigned long us, ms;
xdat = int(XDataH<<8);
ydat = int(YDataH<<8);
xdat |=int(XDataL);
ydat |=int(YDataL);
//int between 0-1023, with 5V/1024 = 0.0049 V/unit
ls = analogRead(lsPin);
//Calculate time elapsed between measurements
ms = millis();
us = micros();
t_ms = ms-ms_start;
t_us = us-us_start;
t = t_ms;
//Convert datatypes to string objects and combine
//us can always be divided by 4, so accurate to a resolution of 4 us
String d1 = String(t_ms);
String d2 = String(t_us);
String d3 = String(xdat);
String d4 = String(ydat);
String d5 = String(ls);
data = d2+","+d3+","+d4+","+d5;
//Increment time
delay(dt);
}
Processing code
/* BEP experiment
Communicates with arduino to conduct experiment
Receives and stores data
/
/ DATA PROTOCOL
data_rx
R start measuring
S do screendump
D device is ready
F measurement finished
/
import processing.serial.;
import controlP5.*;
//Serial COMM
Serial arduino;
String data_rx, data_tx;
String parseChar = ".";
//GUI
ControlP5 cp5;
Textfield txtfldDistance, txtfldSpeed, txtfldTs, txtfldN,
txtfldFl, txtfldBron, txtfldPattern, txtfldTrun;
Button btnRun, btnStop, btnFrame;
//File I/O
PrintWriter writer;
String path;
//Runtime variables
int run = 0;
int createWriter = 0;
int frameCapture = 0;
int frameDisplay = 0;
//Time management
String timestamp;
int ms, ms_start;
final int frameX = 30;
final int frameY = 30;
void setup() {
frameRate(60);
time();
//Create GUI
textSize(20);
size(360,660);
//Create textboxes
cp5 = new ControlP5(this);
txtfldDistance = cp5.addTextfield("Distance[m]:")
.setPosition(30, 30)
.setSize(70, 30)
.setAutoClear(false)
.setText("0.5");
txtfldSpeed = cp5.addTextfield("Speed[rev/s]:")
.setPosition(30, 90)
.setSize(70, 30)
.setAutoClear(false);
txtfldTs = cp5.addTextfield("t_s[ms]")
.setPosition(30, 150)
.setSize(70, 30)
.setAutoClear(false)
.setText("10");
txtfldTrun = cp5.addTextfield("t_run[s]")
.setPosition(30, 210)
.setSize(70, 30)
.setAutoClear(false);
txtfldFl = cp5.addTextfield("f[mm]")
.setPosition(130, 30)
.setSize(70, 30)
.setAutoClear(false)
.setText("14");
txtfldBron = cp5.addTextfield("Bron[Watt]")
.setPosition(130, 90)
.setSize(70, 30)
.setAutoClear(false)
.setText("40");
txtfldPattern = cp5.addTextfield("Pattern[mm]")
.setPosition(130, 150)
.setSize(70, 30)
.setAutoClear(false)
.setText("random");
txtfldN = cp5.addTextfield("n")
.setPosition(130, 210)
.setSize(70, 30)
.setAutoClear(false)
.setText("1");
btnRun = cp5.addButton("Run")
.setPosition(230, 270)
.setSize(50,30)
.lock();
btnStop = cp5.addButton("Stop")
.setPosition(150, 270)
.setSize(50,30)
.lock();
btnFrame = cp5.addButton("Frame_Capture")
.setPosition(30, 270)
.setSize(90,30)
.lock();
//Create Serial COMM object
print(timestamp+"SERIAL PORT: ");
println(Serial.list());
// List all the available serial ports:
//arduino = new Serial(this, Serial.list()[2], 115200);
arduino = new Serial(this, Serial.list()[0], 115200);
arduino.clear();
arduino.bufferUntil('.');
}
void draw() {
time();
Frame_Capture();
display_frame();
if (frameDisplay == 1){
display_frame();
frameDisplay = 0;
println(timestamp+"---------------------");
}
}
int n = 0;
int[] frame_capture_data = new int[900];
void serialEvent(Serial arduino){
if (arduino.available() > 0){
//Parse serial data until '.'
data_rx = arduino.readStringUntil('.');
//Remove CR, LF and '.' from buffer
data_rx = data_rx.substring(2, data_rx.length()-1);
//print(n+":");
//println(data_rx);
if(data_rx.equals("Data transfer finished")){
println(timestamp+"Data transfer finished.");
println(timestamp+"Generating visual.");
frameCapture = 0;
frameDisplay = 1;
n = 0;
//unlock textfields
txtfldSpeed.unlock();
txtfldDistance.unlock();
txtfldTs.unlock();
txtfldBron.unlock();
txtfldPattern.unlock();
txtfldFl.unlock();
txtfldN.unlock();
btnRun.unlock();
btnStop.unlock();
btnFrame.unlock();
}else if(data_rx.equals("Ready")){
println(timestamp+"Device is ready.");
println(timestamp+"---------------------");
//unlock textfields
btnRun.unlock();
btnStop.unlock();
btnFrame.unlock();
}else if(data_rx.equals("Initialized")){
println(timestamp+"Device is initialized.");
}else if(data_rx.equals("Measurement finished")){
println(timestamp+"Measurement completed.");
Stop();
}else if(data_rx.equals("Frame capture")){
println(timestamp+"Frame capture transfer started.");
frameCapture = 1;
}else if(data_rx.equals("Frame capture failed")){
println(timestamp+"Frame capture failed. Try again.");
println(timestamp+"---------------------");
//unlock textfields
txtfldSpeed.unlock();
txtfldDistance.unlock();
txtfldTs.unlock();
txtfldBron.unlock();
txtfldPattern.unlock();
txtfldFl.unlock();
txtfldN.unlock();
btnRun.unlock();
btnStop.unlock();
btnFrame.unlock();
}else if(data_rx.contains("SQUAL")){
print(timestamp+"SQUAL: ");
println(data_rx.substring(5,data_rx.length()));
}else if(data_rx.equals("Time constant error")){
print(timestamp+"TIME CONSTANT ERROR");
}else if(frameCapture == 1 && n < 900){
frame_capture_data[n] = int(data_rx);
n++;
}else if(run == 1){
//print(data_rx);
writer.print(data_rx);
}
}
}
public void Run() {
/* When RUN is pressed program starts to run */
//Read value to determine path
float speed = float(txtfldSpeed.getText());
float distance = float(txtfldDistance.getText());
int t_s = int(txtfldTs.getText());
int bron = int(txtfldBron.getText());
int fl = int(txtfldFl.getText());
String pattern = txtfldPattern.getText();
String date = day()+"-"+month();
int n = int(txtfldN.getText());
// Create CSV data file, showing the results from experiment
if (speed > 0 && distance > 0){
if (createWriter == 0){
//Creating objects for writing to file
path = "data/"+date+"/x="+distance+"/"+"x="+distance+"_v="+speed+
"_ts="+t_s+"_f="+fl+"_bron="+bron+"_pat="+pattern+"_n="+n+".csv";
writer = createWriter(path);
//Runtime variables
createWriter = 1;
run = 1;
ms_start = millis();
//Transmit t_s en t_run
arduino.write("dt"+txtfldTs.getText());
arduino.write(parseChar);
arduino.write("trun"+int(txtfldTrun.getText())*1000);
arduino.write(parseChar);
//Transmit starting char to arduino
arduino.write("Run");
arduino.write(parseChar);
//Header
//writer.println("t_ard_ms,t_ard_us,dx,dy,ls");
//lock textfields
txtfldSpeed.lock();
txtfldDistance.lock();
txtfldTs.lock();
txtfldBron.lock();
txtfldPattern.lock();
txtfldFl.lock();
txtfldN.lock();
btnRun.lock();
btnStop.lock();
btnFrame.lock();
println(timestamp+"PROGRAM INITIATED");
println(timestamp+"File stored at: "+path);
}
//ERROR messages
} else if (speed <= 0 && distance <= 0){
println(timestamp+"ERROR: INVALID SPEED AND DISTANCE");
} else if (speed <= 0){
println(timestamp+"ERROR: INVALID SPEED");
} else if (distance <= 0){
println(timestamp+"ERROR: INVALID DISTANCE ");
} else if(txtfldSpeed.getText().equals("")){
println(timestamp+"ERROR: Enter paramaters.");
}
}
public void Stop() {
/* When STOP is pressed program terminates and writes to file */
if (createWriter == 1){
//Write to file and close stream
writer.flush();
writer.close();
//Runtime variables
run = 0;
createWriter = 0;
//unlock textfields
txtfldSpeed.unlock();
txtfldDistance.unlock();
txtfldTs.unlock();
txtfldBron.unlock();
txtfldPattern.unlock();
txtfldFl.unlock();
txtfldN.unlock();
btnRun.unlock();
btnStop.unlock();
btnFrame.unlock();
txtfldN.setText(str(int(txtfldN.getText())+1));
if (int(txtfldN.getText()) > 5){
txtfldN.setText("1");
txtfldSpeed.clear();
}
println(timestamp+"Data written to file.");
println(timestamp+"---------------------");
}
}
public void Frame_Capture() {
arduino.write("Frame capture run");
arduino.write(parseChar);
//lock textfields
txtfldSpeed.lock();
txtfldDistance.lock();
txtfldTs.lock();
txtfldBron.lock();
txtfldPattern.lock();
txtfldFl.lock();
txtfldN.lock();
btnRun.lock();
btnStop.lock();
btnFrame.lock();
}
void display_frame(){
int[] frame1 = new int[225];
int[] frame2 = new int[255];
int x = 30;
int y = 320;
//resolutie 10x10
int s = 10; // size of pixel, i.e. side lengths
//Max res is 30x30
int sz = 10;
int res = 30;
for (int i = 0; i < 15; i++){
for (int m = 0; m < 15; m++){
frame1[15*i+m] = frame_capture_data[30*i+m];
frame2[15*i+m] = frame_capture_data[30*i+m+15];
}
}
//for (int i = 0; i < res*res; i++){
//Commented by Daan:
//for (int j = 0; j < res; j++){ // j resembles the column index.
// for (int k = 0; k < res; k++){ // k resembles the row index
// //fill(map(frame_capture_data[30*j+k],0,63,0,255));
// //frame_capture_data[30*j+k] = 300; // test to see how the pixel values can be manipulated
// fill(float(frame_capture_data[30*j+k]));
// rect(x+j*10, y+300-k*10, s, s);
// //println(frame_capture_data[30*j+k]);
// }
//}
for( int i = 0; i < 900; i++ )
{
fill( map(frame_capture_data[i], 0, 63, 0, 255) ); // Convert from ADNS greyscale to 0 - 255 grey scale format.
rect(x + (i / frameX * sz), // Each rect() is a rectangle that represents a pixel. I.e. width and height of each pixel is "sz".
y +300 - (i % frameY * sz),
sz, sz);
// //rect(off_x + (i % frameX * sz), // Each rect() is a rectangle that represents a pixel. I.e. width and height of each pixel is "sz".
// //off_y + (i / frameY * sz),
// //sz, sz);
}
fill(255,0,0);
rect(x+3*10, y+300-8*10, s, s); // this is red test dot, j = 3 (column), k = 8 (row).
// I.e. this is the 30*3 + 8 = 98 th pixel in frame stream from sensor.
}
public void time(){
/* Keeps track of time
Creates timestamp for messages*/
String h = str(hour());
String m = str(minute());
String s = str(second());
if (int(h) < 10){
h = "0"+h;
} else if(int(m) < 10){
m = "0"+m;
} else if(int(s) < 10){
s = "0"+s;
}
timestamp = "["+h+":"+m+":"+s+"] ";
}

Arduino LED Controller hanging at startup

I'm trying to code an LED controller that controls the intensity via PWM. However, my issue is that I can't even get to the loop portion, it seems to hang at when I declare my class. I've tried checking to see if any of my functions in my class are causing the issues, but since I can't even get to loop, there must be something wrong within the class. I've written the class and placed it into a library called LED.
The code is somewhat long, but here it is:
#ifndef LED_H
#define LED_H
#include <LiquidCrystal.h>
#include <Button.h>
#include <EEPROM.h>
#include <TimeLib.h>
#include <PWM.h>
class LED
{
public:
LED();
int read_encoder(); //Reads rotary encoder
void clearLCD();
void setAllLed();
void printLCD();
void setOneLed(int);
int setLed(int, // current time in minutes
int, // pin for this channel of LEDs
int, // start time for this channel of LEDs
int, // photoperiod for this channel of LEDs
int, // fade duration for this channel of LEDs
int, // max value for this channel
bool // true if the channel is inverted
);
void menuWizard();
int subMenuWizard(int, int, bool, bool);
void displayMainMenu();
void printMins(int, bool);
void printHMS(byte,byte,byte);
long EEPROMReadlong(long);
void EEPROMWritelong(int, long);
bool pressSelect();
bool pressBack();
void rotateCheck(int&, int, int);
//variables for the LED channels
int minCounter = 0; // counter that resets at midnight.
int oldMinCounter = 0; // counter that resets at midnight.
int ledPins[5]={2,3,5,6,7};
int ledVal[5]={0,0,0,0,0};
// Variables making use of EEPROM memory:
int variablesList[20];
bool invertedLEDs[5]={false,false,false,false,false};
//Backlight Variables
unsigned long backlightIdleMs = 0;
private:
};
#endif // LED_H
And here is the .cpp file:
#define LCD_RS 35 // RS pin
#define LCD_ENABLE 34 // enable pin
#define LCD_DATA4 33 // d4 pin
#define LCD_DATA5 32 // d5 pin
#define LCD_DATA6 31 // d6 pin
#define LCD_DATA7 30 // d7 pin
#define LCD_BACKLIGHT 9 // backlight pin
// Backlight config
#define BACKLIGHT_DIM 10 // PWM value for backlight at idle
#define BACKLIGHT_ON 70 // PWM value for backlight when on
#define BACKLIGHT_IDLE_MS 10000 // Backlight idle delay
#define ENC_A 14
#define ENC_B 15
#define ENC_PORT PINC
#include <LiquidCrystal.h>
#include <Button.h>
#include <EEPROM.h>
#include <TimeLib.h>
#include <PWM.h>
#include "LED.h"
LiquidCrystal lcd(LCD_RS, LCD_ENABLE, LCD_DATA4, LCD_DATA5, LCD_DATA6, LCD_DATA7);
Button goBack=Button(12, PULLDOWN);
Button select=Button(13, PULLDOWN);
LED::LED()
{
InitTimersSafe();
pinMode(LCD_BACKLIGHT, OUTPUT);
lcd.begin(16, 2);
digitalWrite(LCD_BACKLIGHT, HIGH);
lcd.print("sEx LED, V1");
clearLCD();
delay(5000);
analogWrite(LCD_BACKLIGHT, BACKLIGHT_DIM);
if (variablesList[0] > 1440 || variablesList[0] < 0) {
variablesList[0] = 720; // minute to start this channel.
variablesList[1] = 400; // photoperiod in minutes for this channel.
variablesList[2] = 100; // max intensity for this channel, as a percentage
variablesList[3] = 100; // duration of the fade on and off for sunrise and sunset for
// this channel.
variablesList[4] = 720;
variablesList[5] = 400;
variablesList[6] = 100;
variablesList[7] = 100;
variablesList[8] = 720;
variablesList[9] = 400;
variablesList[10] = 100;
variablesList[11] = 100;
variablesList[12] = 720;
variablesList[13] = 400;
variablesList[14] = 100;
variablesList[15] = 100;
variablesList[16] = 720;
variablesList[17] = 400;
variablesList[18] = 100;
variablesList[19] = 100;
}
else {
variablesList[0] = EEPROMReadlong(0); // minute to start this channel.
variablesList[1] = EEPROMReadlong(4); // photoperiod in minutes for this channel.
variablesList[2] = EEPROMReadlong(8); // max intensity for this channel, as a percentage
variablesList[3] = EEPROMReadlong(12); // duration of the fade on and off for sunrise and sunset for
// this channel.
variablesList[4] = EEPROMReadlong(16);
variablesList[5] = EEPROMReadlong(20);
variablesList[6] = EEPROMReadlong(24);
variablesList[7] = EEPROMReadlong(28);
variablesList[8] = EEPROMReadlong(32);
variablesList[9] = EEPROMReadlong(36);
variablesList[10] = EEPROMReadlong(40);
variablesList[11] = EEPROMReadlong(44);
variablesList[12] = EEPROMReadlong(48);
variablesList[13] = EEPROMReadlong(52);
variablesList[14] = EEPROMReadlong(56);
variablesList[15] = EEPROMReadlong(60);
variablesList[16] = EEPROMReadlong(64);
variablesList[17] = EEPROMReadlong(68);
variablesList[18] = EEPROMReadlong(72);
variablesList[19] = EEPROMReadlong(76);
}
}
void LED::printLCD(){lcd.print("test");clearLCD();delay(2000);lcd.print("testing");clearLCD();}
bool LED::pressSelect(){
if (select.uniquePress()){return 1;}
else {return 0;}
}
bool LED::pressBack(){
if (goBack.uniquePress()){return 1;}
else {return 0;}
}
void LED::clearLCD(){
lcd.clear();
}
void LED::displayMainMenu(){
oldMinCounter = minCounter;
minCounter = hour() * 60 + minute();
for (int i=0;i<17;i=i+4){
if (variablesList[i+3] > variablesList[i+1] / 2 && variablesList[i+1] > 0) {
variablesList[i+3] = variablesList[i+1] / 2;
}
if (variablesList[i+3] < 1) {
variablesList[i+3] = 1;
}
}
//check & set any time functions
if (minCounter > oldMinCounter) {
lcd.clear();
}
lcd.setCursor(0, 0);
printHMS(hour(), minute(), second());
lcd.setCursor(0, 1);
lcd.print(ledVal[0]);
lcd.setCursor(4, 1);
lcd.print(ledVal[1]);
lcd.setCursor(8, 1);
lcd.print(ledVal[2]);
}
int LED::read_encoder()
{
static int enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
static int old_AB = 0;
/**/
old_AB <<= 2; //remember previous state
old_AB |= ( ENC_PORT & 0x03 ); //add current state
return ( enc_states[( old_AB & 0x0f )]);
}
int LED::setLed(int mins, // current time in minutes
int ledPin, // pin for this channel of LEDs
int start, // start time for this channel of LEDs
int period, // photoperiod for this channel of LEDs
int fade, // fade duration for this channel of LEDs
int ledMax, // max value for this channel
bool inverted // true if the channel is inverted
) {
int val = 0;
//fade up
if (mins > start || mins <= start + fade) {
val = map(mins - start, 0, fade, 0, ledMax);
}
//fade down
if (mins > start + period - fade && mins <= start + period) {
val = map(mins - (start + period - fade), 0, fade, ledMax, 0);
}
//off or post-midnight run.
if (mins <= start || mins > start + period) {
if ((start + period) % 1440 < start && (start + period) % 1440 > mins )
{
val = map((start + period - mins) % 1440, 0, fade, 0, ledMax);
}
else
val = 0;
}
if (val > ledMax) {
val = ledMax;
}
if (val < 0) {
val = 0;
}
if (inverted) {
pwmWrite(ledPin, map(val, 0, 100, 255, 0));
}
else {
pwmWrite(ledPin, map(val, 0, 100, 0, 255));
}
return val;
}
void LED::printMins(int mins, //time in minutes to print
bool ampm //print am/pm?
) {
int hr = (mins % 1440) / 60;
int mn = mins % 60;
if (hr < 10) {
lcd.print(" ");
}
lcd.print(hr);
lcd.print(":");
if (mn < 10) {
lcd.print("0");
}
lcd.print(mn);
}
void LED::printHMS (byte hr,
byte mn,
byte sec //time to print
)
{
if (hr < 10) {
lcd.print(" ");
}
lcd.print(hr, DEC);
lcd.print(":");
if (mn < 10) {
lcd.print("0");
}
lcd.print(mn, DEC);
lcd.print(":");
if (sec < 10) {
lcd.print("0");
}
lcd.print(sec, DEC);
}
//EEPROM write functions
long LED::EEPROMReadlong(long address)
{
//Read the 4 bytes from the eeprom memory.
long four = EEPROM.read(address);
long three = EEPROM.read(address + 1);
long two = EEPROM.read(address + 2);
long one = EEPROM.read(address + 3);
//Return the recomposed long by using bitshift.
return ((four << 0) & 0xFF) + ((three << 8) & 0xFFFF) + ((two << 16) & 0xFFFFFF) + ((one << 24) & 0xFFFFFFFF);
}
void LED::EEPROMWritelong(int address, long value)
{
//Decomposition from a long to 4 bytes by using bitshift.
//One = Most significant -> Four = Least significant byte
byte four = (value & 0xFF);
byte three = ((value >> 8) & 0xFF);
byte two = ((value >> 16) & 0xFF);
byte one = ((value >> 24) & 0xFF);
//Write the 4 bytes into the eeprom memory.
EEPROM.write(address, four);
EEPROM.write(address + 1, three);
EEPROM.write(address + 2, two);
EEPROM.write(address + 3, one);
}
void LED::setAllLed(){
int j=0;
for (int i=0;i<17;i=i+4){
int a=i;int b=i+1;int c=i+2;int d=i+3;
ledVal[j] = setLed(minCounter, ledPins[j], variablesList[a], variablesList[b], variablesList[c], variablesList[d], invertedLEDs[j]);
j++;
}
}
void LED::setOneLed(int channel){
int j=channel;
int i=0;
if(channel==1){i+=4;}
if(channel==2){i+=8;}
if(channel==3){i+=12;}
if(channel==4){i+=16;}
int a=i;int b=i+1;int c=i+2;int d=i+3;
ledVal[j] = setLed(minCounter, ledPins[j], variablesList[a], variablesList[b], variablesList[c], variablesList[d], invertedLEDs[j]);
}
void LED::rotateCheck(int& menuCount, int minMenu, int maxMenu){
while (menuCount!=0){
int rotateCount;
rotateCount=read_encoder();
if (rotateCount) {
menuCount+=rotateCount;
if (menuCount<minMenu){menuCount==maxMenu;}
if (menuCount>maxMenu){menuCount==minMenu;}
clearLCD();
}
}
}
void LED::menuWizard(){
int menuCount=1;
String menuList[6]={"Time","LED Max","LED Start","LED End","Fade Length","Ch Override"};
String channelList[5]={"1","2","3","4","5"};
while (menuCount!=0){
rotateCheck(menuCount,1,6);
lcd.setCursor(0, 0);
lcd.print(menuList[menuCount-1]);
clearLCD();
if (goBack.isPressed()){
menuCount=0;
}
if (pressSelect() && menuCount!=0){
int timeMode=1;
int channelCount=0;
bool goBack=0;
while (goBack!=1){
if (menuCount==1){
if (pressSelect()){
timeMode++;
if (timeMode>2){timeMode=1;}
}
int timeAdjDetect=read_encoder();
if (timeMode==1){
if (timeAdjDetect){
if (timeAdjDetect>0){adjustTime(SECS_PER_HOUR);}
if (timeAdjDetect<0) {adjustTime(-SECS_PER_HOUR);}
}
lcd.setCursor(0, 0);
lcd.print("Set Time: Hrs");
lcd.setCursor(0, 1);
printHMS(hour(), minute(), second());
}
else{
if (timeAdjDetect){
if (timeAdjDetect>0){adjustTime(SECS_PER_MIN);}
if (timeAdjDetect<0) {adjustTime(-SECS_PER_MIN);}
}
lcd.setCursor(0, 0);
lcd.print("Set Time: Mins");
lcd.setCursor(0, 1);
printHMS(hour(), minute(), second());
}
clearLCD();
}
else{
rotateCheck(channelCount,0,4);
lcd.setCursor(0,0);
lcd.print("Select Channel");
lcd.setCursor(0,1);
lcd.print(channelList[channelCount]);
clearLCD();
if (pressSelect()){
if (menuCount==2){
subMenuWizard(2,channelCount,0,0);
}
if (menuCount==3){
subMenuWizard(0,channelCount,1,0);
}
if (menuCount==4){
subMenuWizard(1,channelCount,1,1);
}
if (menuCount==5){
subMenuWizard(3,channelCount,1,0);
}
}
}
if (pressBack()){goBack=1;}
}
}
}
for (int i=0;i<20;i++){
int j=0;
EEPROMWritelong(j, variablesList[i]);
j+=4;
}
}
int LED::subMenuWizard(int i, int channel, bool time, bool truetime){
if (channel==1){i=i+4;}
if (channel==2){i=i+8;}
if (channel==3){i=i+12;}
if (channel==4){i=i+16;}
while (!pressBack()){
if (time==0){
rotateCheck(variablesList[i],0,100);
lcd.setCursor(0,0);
lcd.print("Set:");
lcd.setCursor(0,1);
lcd.print(variablesList[i]);
setOneLed(channel);
clearLCD();
}
else{
if (truetime){
rotateCheck(variablesList[i],0,1439);
lcd.setCursor(0,0);
lcd.print("Set:");
lcd.setCursor(0,1);
printMins(variablesList[i] + variablesList[i-1], true);
clearLCD();
}
else {
rotateCheck(variablesList[i],0,1439);
lcd.setCursor(0,0);
lcd.print("Set:");
lcd.setCursor(0,1);
printMins(variablesList[i], true);
clearLCD();
}
setOneLed(channel);
}
}
}
and finally, the .ino file:
#define LCD_BACKLIGHT 9 // backlight pin
#define BACKLIGHT_DIM 10 // PWM value for backlight at idle
#define BACKLIGHT_ON 70 // PWM value for backlight when on
#define BACKLIGHT_IDLE_MS 10000 // Backlight idle delay
#include <LED.h>
//Initialize buttons
int buttonCount = 1;
LED main;
void setup() {
};
void loop() {
/* main.setAllLed();
//turn the backlight off and reset the menu if the idle time has elapsed
if (main.backlightIdleMs + BACKLIGHT_IDLE_MS < millis() && main.backlightIdleMs > 0 ) {
analogWrite(LCD_BACKLIGHT, BACKLIGHT_DIM);
main.clearLCD();
main.backlightIdleMs = 0;
}
if (buttonCount == 1) {
main.displayMainMenu();
}
if (buttonCount == 2) {
main.menuWizard();
buttonCount = 1;
}
*/
main.printLCD();
};
Also, in the loop portion, I've commented the part of code that is intended to run, and I'm running a function that tests to see if I've successfully entered the loop by printing "test" on screen.
I'm using a Mega for this.
LED::LED()
{
InitTimersSafe();
pinMode(LCD_BACKLIGHT, OUTPUT);
lcd.begin(16, 2);
digitalWrite(LCD_BACKLIGHT, HIGH);
lcd.print("sEx LED, V1");
clearLCD();
delay(5000);
analogWrite(LCD_BACKLIGHT, BACKLIGHT_DIM);
You have to understand that this constructor is running when the object is created and that is probably before init() is run from main. So the hardware isn't ready at that point and pinMode and digitalWrite and stuff isn't going to work. The lcd code can't really work there and I bet that is the part that is hanging things.
A constructor should only do things like initialize variables. Any code that relies on the hardware should go into a begin() or init() or whatever method that you can call from setup once it is safe to do those things. The Serial object is a great example of another class that has to do this.

Arduino - Adafruit 16-channel board, how to properly control all channels with less delay?

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);
}

Resources