Related
My goal is to control the position and speed of a Nema 17 stepper motor based on the euler angle of a BNO055 inertial measurement unit. I am using an ESP32 to flash the code via WIFI to rosserial. I am powering the Nema 17 with a 12V power source and the BNO055 with a small external 5V battery pack.
In summary, the stepper motor should move between 0-4100 steps which would be mapped to -90 and 90 degrees of the BNO055's y-axis.
For this, I need to read the output of the BNO055 sensor as often as possible and only change directions of the Nema 17 when the BNO055 has changed position relative to the mapping.
The PROBLEM I am having is that when I incorporate reading the sensor in my code, my motor starts to shake and does not rotate smoothly. I am wondering how I can get both things to work simultaneously (reading sensor and moving nema 17).
PS: I will control speed by calculating a PI control with the BNO055 sensor and adjusting the delayMicroseconds() accordingly... but first thing is to get the readings and motor movement smooth.
Below is a code snippet I am using to debug this problem:
#include <WiFi.h>
#include <ros.h>
#include <Wire.h>
#include <std_msgs/Header.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <HardwareSerial.h>
#include <analogWrite.h>
#include <MultiStepper.h>
#include <AccelStepper.h>
#include <Stepper.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
//////////////////////
// BNO055 //
//////////////////////
Adafruit_BNO055 bno_master = Adafruit_BNO055(55, 0x29);
Adafruit_BNO055 bno_slave = Adafruit_BNO055(55, 0x28);
geometry_msgs::Quaternion Quaternion;
std_msgs::String imu_msg;
#define I2C_SDA 21
#define I2C_SCL 22
TwoWire I2Cbno = TwoWire(0); // I2C connection will increase 6Hz data transmission
float ax_m, ay_m, az_m, ax_s, ay_s, az_s; // accelerometer
float gw_m, gx_m, gy_m, gz_m, gw_s, gx_s, gy_s, gz_s; // gyroscope
float ex_m, ey_m, ez_m, ex_s, ey_s, ez_s; // euler
float qw_m, qx_m, qy_m, qz_m, qw_s, qx_s, qy_s, qz_s; // quaternions
//////////////////////
// WiFi Definitions //
//////////////////////
const char* ssid = "FRITZ!Box 7430 PN"; // Sebas: "WLAN-481774"; Paula: "FRITZ!Box 7430 PN"; ICS: ICS24; Hotel Citadelle Blaye
const char* password = "37851923282869978396"; // Sebas: "Kerriganrocks!1337"; Paula: "37851923282869978396"; ICS: uZ)7xQ*0; citadelle
IPAddress server(192,168,178,112); // ip of your ROS server
IPAddress ip_address;
WiFiClient client;
int status = WL_IDLE_STATUS;
//long motorTimer = 0, getImuDataTimer = 0, millisNew = 0; //millisOld = 0,
//////////////////////
// Stepper motor //
//////////////////////
int stepPin = 4;
int stepPinState = LOW;
int dirPin = 2;
int dirPinState = HIGH;
unsigned long millisOld1 = 0;
unsigned long millisOld2 = 0;
long motorTimer = 1; // in milliseconds
long getImuDataTimer = 10; // in milliseconds
double maxPosition = 4100;
double stepsMoved = 0;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class WiFiHardware {
public:
WiFiHardware() {};
void init() {
// do your initialization here. this probably includes TCP server/client setup
client.connect(server, 11411);
}
// read a byte from the serial port. -1 = failure
int read() {
// implement this method so that it reads a byte from the TCP connection and returns it
// you may return -1 is there is an error; for example if the TCP connection is not open
return client.read(); //will return -1 when it will works
}
// write data to the connection to ROS
void write(uint8_t* data, int length) {
// implement this so that it takes the arguments and writes or prints them to the TCP connection
for(int i=0; i<length; i++)
client.write(data[i]);
}
// returns milliseconds since start of program
unsigned long time() {
return millis(); // easy; did this one for you
}
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int i;
void chatterCallback(const std_msgs::String& msg) {
i = atoi(msg.data);
// s.write(i);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setupWiFi()
{
// WIFI setup
WiFi.begin(ssid, password);
Serial.print("\nConnecting to "); Serial.println(ssid);
uint8_t i = 0;
while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
if(i == 21){
Serial.print("Could not connect to"); Serial.println(ssid);
while(1) delay(500);
}
Serial.print("Ready! Use ");
Serial.print(WiFi.localIP());
Serial.println(" to access client");
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Subscriber<std_msgs::String> sub("message", &chatterCallback);
ros::Publisher pub("imu_data/", &imu_msg);
ros::NodeHandle_<WiFiHardware> nh;
void setup() {
// set the digital pins as outputs
pinMode(stepPin, OUTPUT);
pinMode(dirPin, OUTPUT);
Serial.begin(57600);
setupWiFi();
// I2C connection IMUs
Wire.begin(I2C_SDA, I2C_SCL);
I2Cbno.begin(I2C_SDA, I2C_SCL, 400000);
bno_master.begin();
bno_slave.begin();
// get imu calibrations
uint8_t system, gyro, accel, mg = 0;
bno_master.getCalibration(&system, &gyro, &accel, &mg);
bno_slave.getCalibration(&system, &gyro, &accel, &mg);
bno_master.setExtCrystalUse(true);
bno_slave.setExtCrystalUse(true);
nh.initNode();
nh.advertise(pub);
}
/////////////////////////////
/// GET IMU DATA FUNCTION ///
/////////////////////////////
int get_imu_data(){
imu::Vector<3> Euler_s = bno_slave.getVector(Adafruit_BNO055::VECTOR_EULER); // 100 Hz capacity by BNO055 // IF I COMMENT THIS LINE OUT AND SET VARIABLES BELOW TO SET VALUES, MY MOTOR RUNS PERFECTLY
// Euler
float ex_s = Euler_s.x();
float ey_s = Euler_s.y();
float ez_s = Euler_s.z();
// putting data into string since adding accel, gyro, and both imu data becomes too cumbersome for rosserial buffer size. String is better for speed of data
String data = String(ex_s) + "," + String(ey_s) + "," + String(ez_s) + "!";
int length_data = data.indexOf("!") + 1;
char data_final[length_data + 1];
data.toCharArray(data_final, length_data + 1);
imu_msg.data = data_final;
pub.publish(&imu_msg);
nh.spinOnce();
Serial.println(ey_s);
return ey_s; // ex_s, ey_s, ez_s
}
/////////////////////////////
// MAIN LOOP //
/////////////////////////////
void loop() {
unsigned long currentMillis = millis();
//////////////////
// GET IMU DATA //
//////////////////
if(currentMillis - millisOld2 >= getImuDataTimer)
{
ey_s = get_imu_data();
Serial.print(ey_s);
}
////////////////
// MOVE MOTOR //
////////////////
// later, the direction will depend on the output of ey_s
if((dirPinState == HIGH) && (currentMillis - millisOld1 >= motorTimer))
{
if(stepsMoved <= maxPosition)
{
digitalWrite(dirPin, dirPinState);
millisOld1 = currentMillis; // update time
stepsMoved += 5;
for(int i =0; i<=5; i++)
{
digitalWrite(stepPin, HIGH);
delayMicroseconds(1200); // constant speed
digitalWrite(stepPin, LOW);
}
Serial.println(stepsMoved); // checking
}
else if(stepsMoved > maxPosition)
{
dirPinState = LOW;
millisOld1 = currentMillis; // update time
stepsMoved = 0;
}
}
if((dirPinState == LOW) && (currentMillis - millisOld1 >= motorTimer))
{
if(stepsMoved <= maxPosition)
{
digitalWrite(dirPin, dirPinState);
millisOld1 = currentMillis; // update time
stepsMoved += 5;
for(int i =0; i<=5; i++)
{
digitalWrite(stepPin, HIGH);
delayMicroseconds(1200); // constant speed
digitalWrite(stepPin, LOW);
}
Serial.println(stepsMoved); // checking
}
else if(stepsMoved > maxPosition)
{
dirPinState = HIGH;
millisOld1 = currentMillis; // update time
stepsMoved = 0;
}
}
}
I have tried the AccelStepper.h library but not getting the outputs desired in terms of position control and speed updates.
Arduino's all-in-one loop() is not the correct architecture for controlling real-time systems. Motor control requires rather accurate timing - e.g. looks like you wish to update motor control output with a frequency of 833 Hz (from the 1.2 ms delay) which should then be fairly accurate and stable.
Unfortunately you're not getting anywhere near this, as you're doing a bunch of non-critical stuff in each loop which potentially takes a very long (and undeterministic) amount of time - waiting for the IMU to give you a sample, printing to the serial port, talking to some ROS component, etc. Meanwhile the real-time critical control signal to your motor is waiting for all this to finish before it can do its work. Note that printing a few lines to the serial could already take dozens of milliseconds, so your delayMicroseconds(1200); is analogous to measuring a cut with a caliper and then making the cut with an axe with your eyes closed.
A real-time critical process should execute in its own thread which has higher priority than the non-real-time critical stuff. In your case it should probably run off a timer with a 1.2 ms period. The timer handler should execute with higher priority than all the other stuff, calculate desired output to motor using last received sensor input (i.e. don't go asking the IMU for a fresh reading when it's time to move the motor) and exit.
Then you can run all the other stuff from the loop() in idle priority which simply gets pre-empted when the motor control does its work.
Depending on how critical the accurate timing of IMU input is, you may want to run this also in a separate thread with a priority somewhere between the motor control interrupt and idle (remember to yield some CPU cycles to loop() or it'll starve).
Here is the scenario. I have an esp32, 2 led's and a ios example app I found online here. Currently If I press a button on my esp32 it notifies the ios app to display "1", if pressed again it displays "0". This works perfectly.
The tricky part is that this ios app allows me to send a write command to the esp32. I'd like to make it so if '1' is sent LED A turns ON and LED B turns OFF, then LED A OFF and LED B ON when 0 is sent. I am unable to do this though. Try as I might I can't figure out where in the chain of this project something is wrong. Maybe the code on the esp32 or maybe the app i'm unsure.
Here is my arduino code. (There is more to the code not mentioned, I actually have 4 led's but I only want to turn on 2 certain ones when a write command is sent).
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
bool deviceConnected = false;
bool oldDeviceConnected = false;
boolean oldState = LOW;
uint32_t value = 0;
#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"
class MyServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
deviceConnected = true;
};
void onDisconnect(BLEServer* pServer) {
deviceConnected = false;
}
};
class MyCallbacks: public BLECharacteristicCallbacks {
void onWrite(BLECharacteristic *pCharacteristic) {
std::string rxValue = pCharacteristic->getValue();
if (rxValue.length() > 0) {
Serial.print("Received Value: ");
for (int i = 0; i < rxValue.length(); i++) {
Serial.print(rxValue[i]);
}
Serial.println();
if (rxValue.find("1") != -1) {
digitalWrite(13, HIGH);
digitalWrite(27, LOW);
}
else if (rxValue.find("0") != -1) {
digitalWrite(13, LOW);
digitalWrite(27, HIGH);
}
}
}
};
const int bt1 = 14;
boolean bt1g = true;
int bt1t = 0;
void setup() {
pinMode(13, OUTPUT);
pinMode(15, OUTPUT);
pinMode(33, OUTPUT);
pinMode(27, OUTPUT);
pinMode(bt1, INPUT_PULLUP);
Serial.begin(9600);
BLEDevice::init("ESP32");
pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
BLEService *pService = pServer->createService(SERVICE_UUID);
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY
);
pCharacteristic->addDescriptor(new BLE2902());
pService->start();
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(false);
pAdvertising->setMinPreferred(0x0);
BLEDevice::startAdvertising();
Serial.println("Waiting a client connection to notify...");
}
void loop()
{
if (bt1g) {
if (digitalRead(bt1) == LOW ) {
bt1t = (bt1t + 1) % 2;
Serial.println(bt1t);
bt1g = false;
}
}
if (!bt1g) {
if (digitalRead(bt1) == HIGH) {
bt1g = true;
}
}
if (bt1t == 0) {
digitalWrite(15, LOW);
digitalWrite(33, HIGH);
}
}
boolean newState = digitalRead(15);
if (deviceConnected) {
if (newState != oldState) {
if (newState == LOW) {
pCharacteristic->setValue("1");
}
else {
pCharacteristic->setValue("0");
}
pCharacteristic->notify();
};
oldState = newState;
}
delay(50);
}
It looks like the entire code for the ios app is too long to submit to this post so here is the github
I'm really unsure and stuck in a rut. Any help is appreciated!
Could it be you are not discovering the correct characteristics? It looks like you only have one service and one characteristic.
I've recently purchased this: http://redbearlab.com/bleshield/
I've connected it to my Arduino and I'm trying to run the very first test program that they tell me to run which is the BLE Controller sketch. I've connected it and resolved some original compile errors that I initially got, and now it will upload. When I upload it, my iPhone is unresponsive to the shield. I'm trying to figure out if the problem is in the code or if it's a problem with the shield itself. If it's the code, how could I fix the code? I'm relatively new to Arduino and completely new to making it work with Bluetooth. Here's the entire sketch that the guide told me to download from Github.
BLEControllerSketch.ino
/*
Copyright (c) 2012, 2013 RedBearLab
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <Servo.h>
#include <SPI.h>
#include <EEPROM.h>
#include <boards.h>
#include <RBL_nRF8001.h>
#include "Boards.h"
#define PROTOCOL_MAJOR_VERSION 0 //
#define PROTOCOL_MINOR_VERSION 0 //
#define PROTOCOL_BUGFIX_VERSION 2 // bugfix
#define PIN_CAPABILITY_NONE 0x00
#define PIN_CAPABILITY_DIGITAL 0x01
#define PIN_CAPABILITY_ANALOG 0x02
#define PIN_CAPABILITY_PWM 0x04
#define PIN_CAPABILITY_SERVO 0x08
#define PIN_CAPABILITY_I2C 0x10
// pin modes
//#define INPUT 0x00 // defined in wiring.h
//#define OUTPUT 0x01 // defined in wiring.h
#define ANALOG 0x02 // analog pin in analogInput mode
#define PWM 0x03 // digital pin in PWM output mode
#define SERVO 0x04 // digital pin in Servo output mode
byte pin_mode[TOTAL_PINS];
byte pin_state[TOTAL_PINS];
byte pin_pwm[TOTAL_PINS];
byte pin_servo[TOTAL_PINS];
Servo servos[MAX_SERVOS];
void setup()
{
Serial.begin(57600);
Serial.println("BLE Arduino Slave");
/* Default all to digital input */
for (int pin = 0; pin < TOTAL_PINS; pin++)
{
// Set pin to input with internal pull up
pinMode(pin, INPUT);
digitalWrite(pin, HIGH);
// Save pin mode and state
pin_mode[pin] = INPUT;
pin_state[pin] = LOW;
}
// Default pins set to 9 and 8 for REQN and RDYN
// Set your REQN and RDYN here before ble_begin() if you need
//ble_set_pins(3, 2);
// Set your BLE Shield name here, max. length 10
//ble_set_name("My Name");
// Init. and start BLE library.
ble_begin();
}
static byte buf_len = 0;
void ble_write_string(byte *bytes, uint8_t len)
{
if (buf_len + len > 20)
{
for (int j = 0; j < 15000; j++)
ble_do_events();
buf_len = 0;
}
for (int j = 0; j < len; j++)
{
ble_write(bytes[j]);
buf_len++;
}
if (buf_len == 20)
{
for (int j = 0; j < 15000; j++)
ble_do_events();
buf_len = 0;
}
}
byte reportDigitalInput()
{
if (!ble_connected())
return 0;
static byte pin = 0;
byte report = 0;
if (!IS_PIN_DIGITAL(pin))
{
pin++;
if (pin >= TOTAL_PINS)
pin = 0;
return 0;
}
if (pin_mode[pin] == INPUT)
{
byte current_state = digitalRead(pin);
if (pin_state[pin] != current_state)
{
pin_state[pin] = current_state;
byte buf[] = {'G', pin, INPUT, current_state};
ble_write_string(buf, 4);
report = 1;
}
}
pin++;
if (pin >= TOTAL_PINS)
pin = 0;
return report;
}
void reportPinCapability(byte pin)
{
byte buf[] = {'P', pin, 0x00};
byte pin_cap = 0;
if (IS_PIN_DIGITAL(pin))
pin_cap |= PIN_CAPABILITY_DIGITAL;
if (IS_PIN_ANALOG(pin))
pin_cap |= PIN_CAPABILITY_ANALOG;
if (IS_PIN_PWM(pin))
pin_cap |= PIN_CAPABILITY_PWM;
if (IS_PIN_SERVO(pin))
pin_cap |= PIN_CAPABILITY_SERVO;
buf[2] = pin_cap;
ble_write_string(buf, 3);
}
void reportPinServoData(byte pin)
{
// if (IS_PIN_SERVO(pin))
// servos[PIN_TO_SERVO(pin)].write(value);
// pin_servo[pin] = value;
byte value = pin_servo[pin];
byte mode = pin_mode[pin];
byte buf[] = {'G', pin, mode, value};
ble_write_string(buf, 4);
}
byte reportPinAnalogData()
{
if (!ble_connected())
return 0;
static byte pin = 0;
byte report = 0;
if (!IS_PIN_DIGITAL(pin))
{
pin++;
if (pin >= TOTAL_PINS)
pin = 0;
return 0;
}
if (pin_mode[pin] == ANALOG)
{
uint16_t value = analogRead(pin);
byte value_lo = value;
byte value_hi = value>>8;
byte mode = pin_mode[pin];
mode = (value_hi << 4) | mode;
byte buf[] = {'G', pin, mode, value_lo};
ble_write_string(buf, 4);
}
pin++;
if (pin >= TOTAL_PINS)
pin = 0;
return report;
}
void reportPinDigitalData(byte pin)
{
byte state = digitalRead(pin);
byte mode = pin_mode[pin];
byte buf[] = {'G', pin, mode, state};
ble_write_string(buf, 4);
}
void reportPinPWMData(byte pin)
{
byte value = pin_pwm[pin];
byte mode = pin_mode[pin];
byte buf[] = {'G', pin, mode, value};
ble_write_string(buf, 4);
}
void sendCustomData(uint8_t *buf, uint8_t len)
{
uint8_t data[20] = "Z";
memcpy(&data[1], buf, len);
ble_write_string(data, len+1);
}
byte queryDone = false;
void loop()
{
while(ble_available())
{
byte cmd;
cmd = ble_read();
Serial.write(cmd);
// Parse data here
switch (cmd)
{
case 'V': // query protocol version
{
byte buf[] = {'V', 0x00, 0x00, 0x01};
ble_write_string(buf, 4);
}
break;
case 'C': // query board total pin count
{
byte buf[2];
buf[0] = 'C';
buf[1] = TOTAL_PINS;
ble_write_string(buf, 2);
}
break;
case 'M': // query pin mode
{
byte pin = ble_read();
byte buf[] = {'M', pin, pin_mode[pin]}; // report pin mode
ble_write_string(buf, 3);
}
break;
case 'S': // set pin mode
{
byte pin = ble_read();
byte mode = ble_read();
if (IS_PIN_SERVO(pin) && mode != SERVO && servos[PIN_TO_SERVO(pin)].attached())
servos[PIN_TO_SERVO(pin)].detach();
/* ToDo: check the mode is in its capability or not */
/* assume always ok */
if (mode != pin_mode[pin])
{
pinMode(pin, mode);
pin_mode[pin] = mode;
if (mode == OUTPUT)
{
digitalWrite(pin, LOW);
pin_state[pin] = LOW;
}
else if (mode == INPUT)
{
digitalWrite(pin, HIGH);
pin_state[pin] = HIGH;
}
else if (mode == ANALOG)
{
if (IS_PIN_ANALOG(pin)) {
if (IS_PIN_DIGITAL(pin)) {
pinMode(PIN_TO_DIGITAL(pin), LOW);
}
}
}
else if (mode == PWM)
{
if (IS_PIN_PWM(pin))
{
pinMode(PIN_TO_PWM(pin), OUTPUT);
analogWrite(PIN_TO_PWM(pin), 0);
pin_pwm[pin] = 0;
pin_mode[pin] = PWM;
}
}
else if (mode == SERVO)
{
if (IS_PIN_SERVO(pin))
{
pin_servo[pin] = 0;
pin_mode[pin] = SERVO;
if (!servos[PIN_TO_SERVO(pin)].attached())
servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin));
}
}
}
// if (mode == ANALOG)
// reportPinAnalogData(pin);
if ( (mode == INPUT) || (mode == OUTPUT) )
reportPinDigitalData(pin);
else if (mode == PWM)
reportPinPWMData(pin);
else if (mode == SERVO)
reportPinServoData(pin);
}
break;
case 'G': // query pin data
{
byte pin = ble_read();
reportPinDigitalData(pin);
}
break;
case 'T': // set pin digital state
{
byte pin = ble_read();
byte state = ble_read();
digitalWrite(pin, state);
reportPinDigitalData(pin);
}
break;
case 'N': // set PWM
{
byte pin = ble_read();
byte value = ble_read();
analogWrite(PIN_TO_PWM(pin), value);
pin_pwm[pin] = value;
reportPinPWMData(pin);
}
break;
case 'O': // set Servo
{
byte pin = ble_read();
byte value = ble_read();
if (IS_PIN_SERVO(pin))
servos[PIN_TO_SERVO(pin)].write(value);
pin_servo[pin] = value;
reportPinServoData(pin);
}
break;
case 'A': // query all pin status
for (int pin = 0; pin < TOTAL_PINS; pin++)
{
reportPinCapability(pin);
if ( (pin_mode[pin] == INPUT) || (pin_mode[pin] == OUTPUT) )
reportPinDigitalData(pin);
else if (pin_mode[pin] == PWM)
reportPinPWMData(pin);
else if (pin_mode[pin] == SERVO)
reportPinServoData(pin);
}
queryDone = true;
{
uint8_t str[] = "ABC";
sendCustomData(str, 3);
}
break;
case 'P': // query pin capability
{
byte pin = ble_read();
reportPinCapability(pin);
}
break;
case 'Z':
{
byte len = ble_read();
byte buf[len];
for (int i=0;i<len;i++)
buf[i] = ble_read();
Serial.println("->");
Serial.print("Received: ");
Serial.print(len);
Serial.println(" byte(s)");
Serial.print(" Hex: ");
for (int i=0;i<len;i++)
Serial.print(buf[i], HEX);
Serial.println();
}
}
// send out any outstanding data
ble_do_events();
buf_len = 0;
return; // only do this task in this loop
}
// process text data
if (Serial.available())
{
byte d = 'Z';
ble_write(d);
delay(5);
while(Serial.available())
{
d = Serial.read();
ble_write(d);
}
ble_do_events();
buf_len = 0;
return;
}
// No input data, no commands, process analog data
if (!ble_connected())
queryDone = false; // reset query state
if (queryDone) // only report data after the query state
{
byte input_data_pending = reportDigitalInput();
if (input_data_pending)
{
ble_do_events();
buf_len = 0;
return; // only do this task in this loop
}
reportPinAnalogData();
ble_do_events();
buf_len = 0;
return;
}
ble_do_events();
buf_len = 0;
}
Any and all help is greatly appreciated.
I've been working with these quite a bit in the last 8 months, so I'll do what I can to help. I don't have the reputation to comment and ask for clarification on specific things, so I'm just gonna try to cover everything I can.
Let's first lay out a few things:
Because your sketch uploaded I'm assuming that you have added all of the necessary libraries to Arduino. If you haven't, I don't know why it is uploading, but be sure to do that.
The problem almost certainly won't be with the Arduino code. I ran the code you provided, as well as the sketch provided by RBL (Red Bear Lab) that is on my computer. I was able to connect to my iPhone using both sketches.
I'm going to lay out everything I think could potentially be the source of the problem:
Make sure that all of your header pins (you should have a bunch of headers sticking out of the board in rows of 3 next to the digital pins) are connected correctly, as RBL shows in their instructions. If you want me to provide a picture of mine I can do that.
Make sure that the white power light is on on your shield. If it isn't, power isn't getting to the shield itself.
Be sure that you actually have bluetooth enabled on your phone.
You didn't mention that you downloaded the app. Be sure to do that (it is called "BLE Controller" by RedBear), as you cannot connect to the iPhone without the app (Apple's bluetooth menu will not show the BLE shield).
If you have downloaded the app, be sure that you have selected the correct setting from the choices using the button on the top left of the screen (3 lines on top of each other). For the sketch you provided, you should select BLE Controller.
If you have tried everything and nothing else is working, try one of the other sketches provided by RBL, such as SimpleChat. This uses the serial monitor on Arduino to communicate back and forth with the iPhone. If this doesn't work, upload a picture of your specific shield (the top of it) so I can take a look at it. Best of luck.
I've not been programming for long and I just want to expand from electronic engineering with an Arduino UNO board.
I've started a new project based on the Secret Knock Detecting Door Lock by Steve Hoefer on Grathio and I'd like to implement the following:
(http://grathio.com/2009/11/secret_knock_detecting_door_lock/)
(http://grathio.com/assets/secret_knock_detector.pde)
Implementation
If the global value equals 0 and the valid knock patter is true then flash a yellow LED 4 times using millis rather than delay so that it can still 'listen'.
If another valid knock pattern is not heard within 6 seconds it will time out and reset global to 0 so that it can acknowledge the initial true pattern and flash the yellow LED.
If another valid knock pattern is heard withing 6 seconds, increment a counter.
If the counter equals 1, wait for another valid knock pattern and if true within 6 seconds, increment the counter again and don't flash the yellow LED.
Otherwise, time out and reset all values.
And so on until if the counter is greater than or equal to 4 trigger the master LED array.
Once is gets to 4 successful knocks, I'd like it to trigger the master LED array I've built.
Problems
This project was inspired by the test panels used on passenger airplanes. I've seen them a lot and thought it would be a good place to start and learn about timing.
There are a few problems as I don't wish to reset millis() every time and I'm using a button rather than the boolean within the knock detection script so I don't get lost in the code.
I understand this won't respond 50 seconds later and it's a beginners mistake but proves what I've got if I hold down the button. The code below also doesn't have a time out after the 1st digitalRead HIGH or true boolean (I am struggling with this).
Arduino sketch
int inPin = 2; // input pin switch
int outPin = 3; // output pin LED
long currentTime = 0; // counter
long nextTime = 0; // counter
long lastTime = 0; // counter
int patternCounter = 0; // build up
int globalValue = 0; // lock out
int breakIn = 0; // waste of time?
void setup()
{
pinMode(inPin, INPUT);
pinMode(outPin, OUTPUT);
Serial.begin(9600);
Serial.println("GO");
}
void loop(){
// boolean true, switch just for testing
if (digitalRead(inPin)==HIGH&&globalValue==0&&breakIn==0) {
Serial.println("CLEARED 1st");
delay (500); // flood protection
globalValue++;
breakIn++;
if (globalValue>0&&breakIn>0){
currentTime = millis(); // start a 'new' counter and 'listen'
if (currentTime<6000) { // less than
if (digitalRead(inPin)==HIGH) { // and true
Serial.println("CLEARED 2nd"); // cleared the stage
delay (500); // flood protection
patternCounter++;
} // if counter less
} // if true or high
if (currentTime>6000) {
Serial.println("TIMEOUT waiting 2nd"); // timed out
globalValue = 0;
patternCounter = 0;
breakIn = 0;
} // if more than
} // global master
}
// 3rd attempt
if (globalValue==1&&patternCounter==1){ // third round
nextTime = millis(); // start a 'new' counter and 'listen'
if (nextTime<6000) { // less than
if (digitalRead(inPin)==HIGH) { // and true
Serial.println("CLEARED 3rd");
delay (500); // flood protection
patternCounter++;
} // if counter less
} // if true or high
if (nextTime>6000) {
Serial.println("TIMEOUT waiting 3rd"); // timed out
globalValue = 0;
patternCounter = 0;
} // if more than
} // global master
// 4th attempt and latch
if (globalValue==1&&patternCounter==2){ // last round
lastTime = millis(); // start a 'new' counter and 'listen'
if (lastTime<6000) { // less than
if (digitalRead(inPin)==HIGH) { // and true
digitalWrite(outPin, HIGH); // LED on
Serial.println("CLEARED 4th ARRAY"); // cleared the stage
delay(500); // flood protection
} // true or high
} // counter
if (lastTime>6000) {
Serial.println("TIMEOUT waiting 4th"); // timed out
globalValue = 0;
patternCounter = 0;
} // if more than
} // global and alarm
} // loop end
That's the current sketch, I understand the counters I've used are near pointless.
Any help would be greatly appreciated!
That is a lot to wade through so I may not understand your question but the bit of code below stands out as a problem:
currentTime = millis(); // start a 'new' counter and 'listen'
if (currentTime<6000) { // less than
.....
}
Do you understand that there is no "resetting" of millis() possible and that is merely a function that returns the number of milliseconds since the program launched? It will continue to increase as long as the program is running (until it rolls over but that is a separate problem). So in the above code 'currentTime' is only going to be < 6000 very, very briefly (6 seconds) and then never again (except for the rollover condition where millis resets).
So a typical way millis() is used to track time is, in setup, to store it's current value into a variable and add your timeout period value to it:
// timeoutAmount is defined at head of program. Let's say it is 6000 (6 seconds)
nextUpdate = millis() + timeoutAmount;
Then in loop you can do the check:
if (millis() >= nextUpdate){
nextUpdate = millis() + timeoutAmount; // set up the next timeout period
// do whatever you want to do
}
Also be careful using delay() - it is easy to use for flow control but for any program with more than one thing going on it can lead to confusing and hard to solve problems.
Oh - there are more sophisticated ways of doing timing using the built-in timers on the chip to trigger interrupts but better to get the hang of things first.
I've come up with the following sketch after playing around with your help.
The sketch will almost do everything I wanted...
When it times out (T/O) after the 1st, 2nd (inCount = 1) or 3rd (inCount = 2) button press, I'd like it to revert back to the start without having to press it again and loop triggerFlash twice.
Either that or implementing another 'wait and listen' within the time out to move it to the 2nd (inCount = 1) e.t.c. but I think that may cause problems.
I know there's delay used within the flashes but that will be changed to millis(), I'm just trying to get the basic function and understanding.
const int switchPin = 2; // the number of the input pin
const int BswitchPin = 4; // the number of the input pin
const int outPin = 3;
const int thePin = 5;
long startTime; // the value returned from millis when the switch is pressed
long escapeTime; // the value returned from millis when in time out
long duration; // variable to store the duration
int inCount = 0;
int dupe = 0;
void setup()
{
pinMode(switchPin, INPUT);
pinMode(outPin, OUTPUT);
pinMode(thePin, OUTPUT);
digitalWrite(switchPin, HIGH); // turn on pull-up resistor
Serial.begin(9600);
Serial.println("Go");
digitalWrite(outPin, HIGH);
}
void loop()
{
if(inCount==0&&digitalRead(switchPin) == LOW)
{
// here if the switch is pressed
startTime = millis();
while(inCount==0&&digitalRead(switchPin) == LOW)
; // wait while the switch is still pressed
long duration = millis() - startTime;
if (duration<4000) {
Serial.println("1");
triggerFlash();
inCount++;
}
} // master 1
if (inCount>0&&inCount<4&&digitalRead(switchPin) == LOW)
{
// here if the switch is pressed
startTime = millis();
while(inCount>0&&inCount<4&&digitalRead(switchPin) == LOW)
; // wait while the switch is still pressed
long duration = millis() - startTime;
delay(500); // flood protection
if (duration>4000) { // script an escape here - formerly if (while will loop the condition)
Serial.println("T/O");
triggerFlash();
inCount = 0;
}
if (duration<4000) {
dupe = inCount + 1;
Serial.println(dupe);
inCount++;
}
}
if (inCount>=4) {
digitalWrite(thePin, HIGH);
}
} // loop
void triggerFlash() {
int i = 0;
for (i=0; i < 8; i++){
digitalWrite(outPin, LOW);
delay(100);
digitalWrite(outPin, HIGH);
delay(100);
}
}
Any ideas are very appreciated! (edited with improved counting)
The above code is actually WRONG. Please be carefull with millis() as they rollover after some time. it is only long type. So if the millis+timeout is near max(long) and millis() will rollover and start counting from zero, the millis()>=nextupdate will be false even if the timeout actually occurs.
The correct way to do this is:
unsigned long start = millis();
unsigned long timeout = MY_TIMEOUT_HERE;
...
//check if timeout occured
unisgned long now = millis();
unsigned long elapsed = now - start;
if(elapsed > timeout)
//do whatever you need to do when timeout occurs
I just implement Arduino library. hope it help your problem.
I made it to work like setTimeout and setInterval in javascript.
You can download it here, Github
This is example of my code
You can see it in action in Tinkercad
/*
Author : Meng Inventor
Contact : https://www.facebook.com/MLabpage
15 July 2022
*/
#include "simple_scheduler.h"
#define LED1_PIN 7
#define LED2_PIN 6
#define LED3_PIN 5
#define GREEN_LED_PIN 4
Task_list job_queue;
void setup()
{
Serial.begin(115200);
pinMode(LED1_PIN, OUTPUT);
pinMode(LED2_PIN, OUTPUT);
pinMode(LED3_PIN, OUTPUT);
pinMode(GREEN_LED_PIN, OUTPUT);
// setInterval will run repeatly for every given time period (ms)
job_queue.setInterval(blink_green, 1000);
job_queue.setInterval(led1_on, 2000);
}
unsigned long timer = millis();
void loop()
{
job_queue.update();
}
void led1_on(){
digitalWrite(LED1_PIN, HIGH);
job_queue.setTimeout(led1_off, 250); //setTimeout will run once after given time period (ms)
}
void led1_off(){
digitalWrite(LED1_PIN, LOW);
job_queue.setTimeout(led2_on, 250);//setTimeout will run once after given time period (ms)
}
void led2_on(){
digitalWrite(LED2_PIN, HIGH);
job_queue.setTimeout(led2_off, 250);//setTimeout will run once after given time period (ms)
}
void led2_off(){
digitalWrite(LED2_PIN, LOW);
job_queue.setTimeout(led3_on, 250);//setTimeout will run once after given time period (ms)
}
void led3_on(){
digitalWrite(LED3_PIN, HIGH);
job_queue.setTimeout(led3_off, 250);//setTimeout will run once after given time period (ms)
}
void led3_off(){
digitalWrite(LED3_PIN, LOW);
}
void blink_green() {
digitalWrite(GREEN_LED_PIN,HIGH);
job_queue.setTimeout(blink_green_off, 500);
}
void blink_green_off() {
digitalWrite(GREEN_LED_PIN,LOW);
}
I'm doing a project about controlling two sensors (ultrasonic and infrared), managing them with Arduino. The IR receiver has a filter system inside, so it receives at the frequency of 36 kHz. I use the module srf04 to handle the ultrasonic stuff. If I do a program which has to control only one sensor, it works. But I have to interpolate the two signals into one result. So I used protothreads! But it doesn't work... What's the error?
Here is the code:
#include <pt.h>
int iro = 8, iri = 4, us = 12, distanza, us_vcc = 13, ir_vcc = 7;
long durata;
static struct pt pt1, pt2, pt3;
static int irthread(struct pt *pt) {
PT_BEGIN(pt);
while(1) {
PT_WAIT_UNTIL(pt, 1>0);
digitalWrite(iro, HIGH);
delayMicroseconds(9);
digitalWrite(iro, LOW);
delayMicroseconds(9);
}
PT_END(pt);
}
static int usthread(struct pt *pt) {
static unsigned long timer = 0;
PT_BEGIN(pt);
while(1) {
PT_WAIT_UNTIL(pt, millis() - timer > 200);
timer = millis();
pinMode(us, OUTPUT);
digitalWrite(us, LOW);
delayMicroseconds(5);
digitalWrite(us, HIGH);
delayMicroseconds(10);
digitalWrite(us, LOW);
pinMode(us, INPUT);
durata = pulseIn(us, HIGH);
distanza = durata/58;
}
PT_END(pt);
}
static int leggithread(struct pt *pt) {
static unsigned long timer = 0;
PT_BEGIN(pt);
while(1) {
PT_WAIT_UNTIL(pt, millis() - timer > 200);
timer = millis();
Serial.print(distanza);
Serial.print("cm ");
if (digitalRead(iri) == LOW)
Serial.println("ir si");
else
Serial.println("ir no");
}
PT_END(pt);
}
void setup() {
pinMode(iro, OUTPUT);
pinMode(iri, INPUT);
pinMode(us_vcc, OUTPUT);
digitalWrite(us_vcc, HIGH);
pinMode(ir_vcc, OUTPUT);
digitalWrite(ir_vcc, HIGH);
Serial.begin(9600);
PT_INIT(&pt1);
PT_INIT(&pt2);
PT_INIT(&pt3);
}
void loop() {
irthread(&pt1);
usthread(&pt2);
leggithread(&pt3);
}
The single parts of code of each thread works.
Update
I solved my problem (eliminated irthread()) and the code is now like this:
#include <pt.h>
int iro = 8, iri = 4, us = 12, distanza, us_vcc = 13, ir_vcc = 7;
long durata;
static struct pt pt1, pt2;
static int usthread(struct pt *pt) {
static unsigned long timer = 0;
PT_BEGIN(pt);
while(1) {
PT_WAIT_UNTIL(pt, millis() - timer > 200);
timer = millis();
pinMode(us, OUTPUT);
digitalWrite(us, LOW);
delayMicroseconds(5);
digitalWrite(us, HIGH);
delayMicroseconds(10);
digitalWrite(us, LOW);
pinMode(us, INPUT);
durata = pulseIn(us, HIGH);
}
PT_END(pt);
}
static int leggithread(struct pt *pt) {
static unsigned long timer = 0;
PT_BEGIN(pt);
while(1) {
PT_WAIT_UNTIL(pt, millis() - timer > 200);
timer = millis();
distanza = durata/58;
Serial.print(distanza);
Serial.print("cm ");
if(digitalRead(iri) == LOW)
Serial.println("ir si");
else
Serial.println("ir no");
}
PT_END(pt);
}
void setup() {
pinMode(iro, OUTPUT);
tone(iro, 36000);
pinMode(iri, INPUT);
pinMode(us_vcc, OUTPUT);
digitalWrite(us_vcc, HIGH);
pinMode(ir_vcc, OUTPUT);
digitalWrite(ir_vcc, HIGH);
Serial.begin(9600);
PT_INIT(&pt1);
PT_INIT(&pt2);
}
void loop() {
usthread(&pt1);
leggithread(&pt2);
}
Now the problem is the ultrasonic sensor. If I control it in a single program without protothreads it can reach objects to a distance of 3 meters. Now even if I put something at 1 meter the "distanza" is 15 cm max. What is the error?
In irthread() the second argument to macro PT_WAIT_UNTIL always evaluates to true:
PT_WAIT_UNTIL(pt, 1>0);
Thus the program will be stuck in irthread()'s infinite loop, because part of the result of macro PT_WAIT_UNTIL in this case is something like if(!(1>0)) return 0;; the statement return 0 is never called.
It works for usthread() and leggithread() as the second argument is false for the first 200 milliseconds and the variables are set up so it will be false again for another 200 milliseconds after being true for a single time.
Some background information is in How protothreads really work.
The timers in leggithread() and usthread() interferes with each other. They use the same variable, timer. When time is up, after about 200 milliseconds since last time, in, say leggithread(), the variable is reset. It means the condition in the other function, usthread() (that is called right after), will be false even though the condition there was about to be true. Thus at least another 200 milliseconds will pass before usthread() can do work (outputting a 10 microsecond pulse on port 12).
There is no guarantee that both functions will be called. If you are unlucky only one of them may be called if it is a deterministic system (driven from the same clock, the microcontroller's crystal).
It could be random which one is called or there could be some aliasing between several frequencies (for instance, one frequency represented by the number of executed instructions for each loop - that frequency will change when the program is changed).
If you want both leggithread() and usthread() doing work five times per second then they should each have an independent timer, using separate variables, for example, timer1 and timer2.
Why have you put while(1) in your function? Since 1 is always true -
while(1) {
// The code in it will repeat forever
}
// And the Arduino will never get here
Either you put a logic instead of 1 (like while(x > 10), while(task_finished)) or don't put your code in the while statement.
static int usthread(struct pt *pt) {
static unsigned long timer = 0;
PT_BEGIN(pt);
while(1) { // <<<<<<<<< Fault 1
PT_WAIT_UNTIL(pt, millis() - timer > 200);
PT_BEGIN(pt);
while(1) { //<<<<<<<<< Fault 2
PT_WAIT_UNTIL(pt, millis() - timer > 200);
timer = millis();