How should I write angle values into stepper motors similare to servoWriteFloat() for servo motors? - arduino-uno

I am working on a delta robot controlled with stepper motors through arduino uno.
I found a code online that allows me to use Arduino processing IDE but this code was written for servo motors.
So, I am trying to re-adjust this code to work with my stepper motors but I am very new to using arduino and programming so I am facing some difficulties.
I think my main problem is not knowing how to write angle values into stepper motors which is this part of their code:
`
servoWriteFloat(&servo1, t1); //write angles value into the servo
servoWriteFloat(&servo2, t2+10); //write angles value into the servo - the 10 value is due to not perfect alignement of the servo
servoWriteFloat(&servo3, t3+5); //write angles value into the servo - the 5 value is due to not perfect alignement of the servo
`
Here are the component used in this project:
Component used in this project:
Two NEMA 17 stepper motors Frame Size 42x42mm Technique specification: Step Angle: 1.8° – Current /phase: 1.7 A – Resistance /phase: 1.5 Ω – Inductance /phase: 2.8 mH – Holding Torque: 2.2 N.cm – leads: 4 wires – Motor Wight: 0.28 Kg – Motor Length: 40 mm
One NEMA 23 Stepper motor Technique specification: Step Angle: 1.8°– Current /phase: 3 A – Resistance /phase: 1Ω – Inductance /phase: 1.8 mH – Holding Torque: 13 Kg.cm – leads: 8 wires – Motor Wight: 1 Kg – Length: 76 mm
Two A4988 – Stepper Motor Driver Specs:
Minimum operating voltage: 8 V
Maximum operating voltage: 35 V
Continuous current per phase: 1 A
Maximum current per phase: 2 A
Minimum logic voltage: 3 V
Maximum logic voltage: 5.5 V
Microstep resolutions: 1, 1/2, 1/4, 1/8, 1/16
Size: 0.6″ × 0.8″
Weight: 1.3 g
One Drv8825 – Stepper Motor Driver specs
Arduino Shield "NANO CNC Machine V4 Board" Description: There are total 3 ways slots for stepper motor driver modules (Model A4988 or Drv8825 – It sold separately at our store ), and it can drive 3 ways stepper motors each up to 2A. Each way stepper motor has two I/O ports and it means 6 I/O ports can manage 3 stepper motors. It is very convenient to operate.
All connected together to an arduino uno, the connections and the components are almost the same as the ones used in this youtube video
The Following is my attempt to readjust the code to work for stepper motors:
`
#include <Stepper.h>
float xPos = 0; // x axis position
float yPos = 0; // y axis position
float zPos = -190; // z axis position, negative is up, positive is down. Delta bots are usually used up side down...
//float xPos = 78.231;
//float yPos = -78.231;
//float zPos = -377.824;
float t1; //stepper angle t for 'theta', 1 for stepper 1
float t2;
float t3;
int result;
int data = 0;
int serialTeller=0;
#define MAXANGLE 98.73 //maximum angle allowed by the Steppers
#define MINANGLE -42.92 //minimum angle alloweb by the Steppers
// defines pins numbers
const int stepX = 2;
const int dirX = 5;
const int stepY = 3;
const int dirY = 6;
const int stepZ = 4;
const int dirZ = 7;
const int enPin = 8;
void setup() {
// initialize the serial port:
Serial.begin(9600);
// Sets the two pins as Outputs
pinMode(stepX,OUTPUT);
pinMode(dirX,OUTPUT);
pinMode(stepY,OUTPUT);
pinMode(dirY,OUTPUT);
pinMode(stepZ,OUTPUT);
pinMode(dirZ,OUTPUT);
pinMode(enPin,OUTPUT);
digitalWrite(enPin,LOW);
digitalWrite(dirX,HIGH);
digitalWrite(dirY,HIGH);
digitalWrite(dirZ,HIGH);
}
void loop(){
//int variable1 = int(random(100));
//Serial.print(variable1);
char xxPos[4]; // x position taken from processing
char yyPos[4]; // y position taken from processing
char zzPos[4]; // z position taken from processing
char tempo; // temp variable
if (Serial.available() > 12) {
tempo=Serial.read(); //read from serial
int conta=0;
while (tempo != 'X') // wait for the X position
{
tempo=Serial.read();
}
if (Serial.available() >= 11) {
xxPos[0]=Serial.read(); //read X byte
xxPos[1]=Serial.read(); //read X byte
xxPos[2]=Serial.read(); //read X byte
xxPos[3]=Serial.read(); //read X byte
xPos=atoi(xxPos); //transfor bytes in integer
tempo=Serial.read(); //read Y char
yyPos[0]=Serial.read(); //read Y byte
yyPos[1]=Serial.read(); //read Y byte
yyPos[2]=Serial.read(); //read Y byte
yyPos[3]=Serial.read(); //read Y byte
yPos=atoi(yyPos); //transform bytes in integer
tempo=Serial.read(); //read Z Char
zzPos[0]=Serial.read(); //read Z byte
zzPos[1]=Serial.read(); //read Z byte
zzPos[2]=Serial.read(); //read Z byte
zzPos[3]=Serial.read(); //read Z byte
zPos=atoi(zzPos); //transform bytes in integer
}
}
result = delta_calcInverse(xPos, yPos, zPos, t1, t2, t3);
// stepperWriteFloat(&stepX, t1); //How should I rewrite servoWriteFloat for stepper motors?
// stepperWriteFloat(&stepY, t2+10);
// stepperWriteFloat(&stepZ, t3+5);
//Should I write it this way?
digitalWrite(stepX, t1);
digitalWrite(stepY, t2+10);
digitalWrite(stepZ, t2+5);
//or should I write it similare to this ?
//// initialize the stepper library on pins:
//Stepper myStepper1(t1, 2, 5);
//Stepper myStepper2(t2+10, 3, 6);
//Stepper myStepper3(t3+5, 4, 7);
// // set the speed at 60 rpm:
//myStepper1.setSpeed(60);
//// set the speed at 60 rpm:
//myStepper2.setSpeed(60);
//// set the speed at 60 rpm:
//myStepper3.setSpeed(60);
//
//// Serial.println("myStepper1 t1");
// myStepper1.step(t1);
// Serial.println("myStepper1 t2");
// myStepper1.step(t2+10);
// Serial.println("myStepper1 t3");
// myStepper1.step(t3+5);
}
//////////////////// I think this part should remain the same////////////////////
//Note: I copied this part in the same file as the above code because otherwise it gives me and error saying that result is not indicated in this scope//
// print some values in the serial
void printSerial(){
Serial.print(result == 0 ? "ok" : "no");
char buf[10];
dtostrf(xPos, 4, 0, buf);
Serial.print(" X");
Serial.print(buf);
dtostrf(yPos, 4, 0, buf);
Serial.print(" Y");
Serial.print(buf);
dtostrf(zPos, 4, 0, buf);
Serial.print(" Z");
Serial.print(buf);
dtostrf(t1, 6, 2, buf);
Serial.print(" T1");
Serial.print(buf);
dtostrf(t2, 6, 2, buf);
Serial.print(" T2");
Serial.print(buf);
dtostrf(t3, 6, 2, buf);
Serial.print(" T3");
Serial.print(buf);
Serial.println("");
}
The Following is the processing IDE code which I believe works just fine:
//write X value in serial port
xPos="X";
if (XXX > 0)
xPos += '+';
else
xPos += '-';
if (abs(XXX) < 100)
xPos += '0';
if (abs(XXX) < 10)
xPos += '0';
xPos += abs(XXX);
// write Y value in serial port
yPos="Y";
if (YYY > 0)
yPos += '+';
else
yPos += '-';
if (abs(YYY) < 100)
yPos += '0';
if (abs(YYY) < 10)
yPos += '0';
yPos += abs(YYY);
// write Z value in serial port
zPos="Z";
if (ZZZ > 0)
zPos += '+';
else
zPos += '-';
if (abs(ZZZ) < 100)
zPos += '0';
if (abs(ZZZ) < 10)
zPos += '0';
zPos += abs(ZZZ);
// write x,y,z in the serial port
myPort.write(xPos);
myPort.write(yPos);
myPort.write(zPos);
// write x,y,z in the monitor
print(xPos);
print(yPos);
println(zPos);
}
`
I also read this Forum and found out the following: "A stepper - in contrast to a servo - does not know where it is. If you want it to move to an absoulte angle, you must fist determine a starting position - e.g. with an end switch." but I am still not sure how to move on from here, How should I wrtie angle values into stepper motor?
Note: I believe the kinematics code should remain the same in both cases.

Related

Robot not turning on one set of code, but turns on the other similar set of code

Im making an alarm robot using the Sparkfun Redbot for a class project. When the robot detects something in front of it, or detects a drop off, it would turn 90 degrees. It worked on the Autonomous Navigation Test file, but it doesn't turn with the Prototype code file, both Autonomous Navigation functions are the same.
Autonomous Navigation Code
// SparkFun RedBot Library
#include <RedBot.h>
#include <RedBotSoftwareSerial.h>
// create objects using classes in RedBot library
RedBotButton button;
RedBotMotors motors;
RedBotEncoder encoder(A2, 10);
RedBotSensor leftLine(A3);
RedBotSensor centerLine(A6);
RedBotSensor rightLine(A7);
// global variables for buzzer and LED pin numbers
const int buzzer = 9;
const int led = 13;
// global variables for ultrasonic sensor pin numbers
const int TRIG_PIN = A0;
const int ECHO_PIN = A1;
// global variables to keep track of which scenario to demonstrate
int scenario = 1;
boolean started = false;
// variables for wheel encoder counts and motor powers
long leftCount, rightCount;
long prevLeftCount, prevRightCount;
int leftPower, rightPower;
const int motorPower = 150; // change value if needed - used by driveStraight() function
void setup() {
pinMode(buzzer, OUTPUT);
pinMode(led, OUTPUT);
// ultrasonic sensor setup
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
digitalWrite(TRIG_PIN, LOW);
}
void loop() {
checkButton();
if (started == true) {
if (scenario == 1) scenario1();
else if (scenario == 2) scenario2();
else if (scenario == 3) scenario3();
}
}
void scenario1() {
// Scenario 1: Drive autonomously by avoiding lines and obstacles
unsigned long time = millis(); // get current time in milliseconds
unsigned long endTime = time + 30000; // make end time 30 seconds from now
// while current time is less than end time, run demo
while (time < endTime) {
avoidCollision();
checkDropOff();
time = millis(); // check current time again
}
// time's up
motors.stop();
alertSound();
// set global variables for 2nd scenario
scenario = 2;
started = false;
}
void scenario2() {
// add code for Scenario 2
// set global variables for 3rd scenario
scenario = 3;
started = false;
}
void scenario3() {
// add code for Scenario 3
// reset global variables for 1st scenario
scenario = 1;
started = false;
}
//---------------------------------------------------------
void checkDropOff() {
// IR threshold indicating surface drop-off (table edge, hole, etc.)
int dropOff = 950; // adjust value if necessary
// get IR sensor readings
int leftSensor = leftLine.read();
int centerSensor = centerLine.read();
int rightSensor = rightLine.read();
// see if any IR sensors detect drop-off
if (leftSensor > dropOff || centerSensor > dropOff || rightSensor > dropOff) {
// add code to perform (brake, reverse, change direction, etc.)
motors.brake();
delay(1000);
motors.drive(-150); // back up
delay(500);
motors.brake();
pivotAngle(90);
started = false; // allow test to be repeated
}
}
void checkButton() {
if (button.read() == true) {
// reverse value of "started"
started = !started;
// single-blink and single-beep as feedback
digitalWrite(led, HIGH);
tone(buzzer, 3000);
delay(200);
digitalWrite(led, LOW);
noTone(buzzer);
}
}
void alertSound() {
// modify code to play whatever sound pattern you want
for (int i = 0; i < 3; i++) {
tone(buzzer, 4000);
delay(100);
noTone(buzzer);
delay(100); // pause before next beep
}
}
void driveDistance(float distance, int power) {
// use wheel encoders to drive straight for specified distance at specified power
// set initial power for left and right motors
int leftPower = power;
int rightPower = power;
// amount to offset motor powers to drive straight
int offset = 5;
// if negative distance, make powers & offset also negative
if (distance < 0) {
leftPower *= -1;
rightPower *= -1;
offset *= -1;
}
// adjust distance to improve accuracy
float correction = -1.0; // change value based on test results
if (distance > 0) distance += correction;
else if (distance < 0) distance -= correction;
// variables for tracking wheel encoder counts
long leftCount = 0;
long rightCount = 0;
long prevLeftCount = 0;
long prevRightCount = 0;
long leftDiff, rightDiff;
// RedBot values based on encoders, motors & wheels
float countsPerRev = 192.0; // 192 encoder ticks per wheel revolution
float wheelDiam = 2.56; // wheel diameter = 65 mm = 2.56 in
float wheelCirc = PI * wheelDiam; // wheel circumference = 3.14 x 2.56 in = 8.04 in
// based on distance, calculate number of wheel revolutions
float numRev = distance / wheelCirc;
// calculate target encoder count
float targetCount = numRev * countsPerRev;
// reset encoder counters and start driving
encoder.clearEnc(BOTH);
delay(100);
motors.leftDrive(leftPower);
motors.rightDrive(rightPower);
// keeps looping while right encoder count less than target count
while (abs(rightCount) < abs(targetCount)) {
// get current wheel encoder counts
leftCount = encoder.getTicks(LEFT);
rightCount = encoder.getTicks(RIGHT);
// calculate increase in count from previous reading
leftDiff = abs(leftCount - prevLeftCount);
rightDiff = abs(rightCount - prevRightCount);
// store current counts as "previous" counts for next reading
prevLeftCount = leftCount;
prevRightCount = rightCount;
// adjust left & right motor powers to keep counts similar (drive straight)
// if left rotated more than right, slow down left & speed up right
if (leftDiff > rightDiff) {
leftPower = leftPower - offset;
rightPower = rightPower + offset;
}
// else if right rotated more than left, speed up left & slow down right
else if (leftDiff < rightDiff) {
leftPower = leftPower + offset;
rightPower = rightPower - offset;
}
// apply adjusted motor powers
motors.leftDrive(leftPower);
motors.rightDrive(rightPower);
delay(10); // short delay before next reading
}
// target count reached
motors.brake(); // or use: motors.stop()
delay(500); // brief delay to wait for complete stop
}
void pivotAngle(float angle) {
// use wheel encoders to pivot (turn) by specified angle
// set motor power for pivoting
int power = 100; // turn CW
if (angle < 0) power *= -1; // use negative power to turn CCW
// adjust angle to improve accuracy
float correction = -5.0; // change value based on test results
if (angle > 0) angle += correction;
else if (angle < 0) angle -= correction;
// variable for tracking wheel encoder counts
long rightCount = 0;
// RedBot values based on encoders, motors & wheels
float countsPerRev = 192.0; // 192 encoder ticks per wheel revolution
float wheelDiam = 2.56; // wheel diameter = 65 mm = 2.56 in
float wheelCirc = PI * wheelDiam; // wheel circumference = 3.14 x 2.56 in = 8.04 in
float pivotDiam = 6.125; // pivot diameter = distance between centers of wheel treads = 6.125 in
float pivotCirc = PI * pivotDiam; // pivot circumference = 3.14 x 6.125 in = 19.23 in
// based on angle, calculate distance (arc length)
float distance = abs(angle) / 360 * pivotCirc;
// based on distance, calculate number of wheel revolutions
float numRev = distance / wheelCirc;
// calculate target encoder count
float targetCount = numRev * countsPerRev;
// reset encoder counters and start pivoting
encoder.clearEnc(BOTH);
delay(100);
motors.pivot(power);
// keeps looping while right encoder count less than target count
while (abs(rightCount) < abs(targetCount)) {
// get current wheel encoder count
rightCount = encoder.getTicks(RIGHT);
delay(10); // short delay before next reading
}
// target count reached
motors.brake();
delay(250);
// clearEncoders(); // only needed if using driveStraight() or countLine()
}
void avoidLine() {
/* AVOID LINE
To avoid dark line on light surface:
Use high threshold & see if sensors greater than threshold
To avoid light line on dark surface:
Use low threshold & see if sensors less than threshold
Use test readings from line to determine best value for threshold
*/
// adjust value if necessary
int lineThreshold = 800;
// get IR sensor readings
int leftSensor = leftLine.read();
int rightSensor = rightLine.read();
// when either sensor on line, first brake motors
if (leftSensor > lineThreshold || rightSensor > lineThreshold) {
motors.brake();
delay(250);
}
// when both sensors on line, turn around
if (leftSensor > lineThreshold && rightSensor > lineThreshold) {
long rnd = random(750, 1250);
motors.pivot(100);
delay(rnd);
motors.stop();
}
// when line under left sensor only, pivot right
else if (leftSensor > lineThreshold) {
long rnd = random(500, 750);
motors.pivot(100);
delay(rnd);
motors.stop();
}
// when line under right sensor only, pivot left
else if (rightSensor > lineThreshold) {
long rnd = random(500, 750);
motors.pivot(-100);
delay(rnd);
motors.stop();
}
delay(25); // change delay to adjust line detection sensitivity
}
void avoidCollision() {
// set minimum distance between RedBot and obstacle
float minDist = 8.0; // change value as necessary (need decimal)
// measure distance to nearest obstacle
float sensorDist = measureDistance();
// if obstacle is too close, avoid collision
if (sensorDist <= minDist) {
// add code to perform (brake, change direction, etc.)
motors.brake();
pivotAngle(90);
}
else{
motors.drive(120);
}
delay(60); // need min 60ms between ultrasonic sensor readings
}
float measureDistance() {
// declare local variables for function
unsigned long time1;
unsigned long time2;
unsigned long pulse_time;
float dist_cm;
float dist_in;
// trigger ultrasonic signal for minimum 10 microseconds
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
// wait for echo to be received
while (digitalRead(ECHO_PIN) == 0);
// now measure how long echo lasts (pulse time)
time1 = micros(); // get start time in microseconds
while (digitalRead(ECHO_PIN) == 1); // wait until echo pulse ends
time2 = micros(); // get end time
pulse_time = time2 - time1; // subtract to get amount of time
// pulse time of 23200 represents maximum range for sensor
if (pulse_time > 23200) pulse_time = 23200;
/*
CALCULATE DISTANCE TO OBJECT USING PULSE TIME
distance = pulse time * speed of sound / 2
Divide by 2 because pulse time includes signal traveling to object and then back
Speed of sound in air at sea level is approximately 340 m/s
Numbers below are calculated constants for cm or inches
Decimal point in numbers necessary for accurate float calculation
*/
dist_cm = pulse_time / 58.0;
dist_in = pulse_time / 148.0;
// return measurement (dist_cm or dist_in)
return dist_in;
}
Prototype Code:
// SparkFun RedBot Library - Version: Latest
#include <RedBot.h>
#include <RedBotSoftwareSerial.h>
// GLOBAL VARIABLES AND OBJECTS
// create objects using classes in RedBot library
RedBotButton button;
RedBotMotors motors;
RedBotEncoder encoder(A2, 10);
RedBotSensor leftLine(A3);
RedBotSensor centerLine(A6);
RedBotSensor rightLine(A7);
// global variables for buzzer and LED pin numbers
const int buzzer = 9;
const int led = 13;
// global variables for ultrasonic sensor pin numbers
const int TRIG_PIN = A0;
const int ECHO_PIN = A1;
// global variables to keep track of which scenario to demonstrate
int scenario = 1;
boolean started = false;
// variables for wheel encoder counts and motor powers
long leftCount, rightCount;
long prevLeftCount, prevRightCount;
int leftPower, rightPower;
const int motorPower = 150; // change value if needed - used by driveStraight() function
int speaker = 9;
// SETUP FUNCTION
void setup() {
// put your setup code here, to run once:
pinMode(buzzer, OUTPUT);
pinMode(led, OUTPUT);
pinMode(speaker, OUTPUT);
// ultrasonic sensor setup
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
digitalWrite(TRIG_PIN, LOW);
}
// LOOP FUNCTION
void loop() {
checkButton();
if (started == true) {
if (scenario == 1) scenario1();
else if (scenario == 2) scenario2();
else if (scenario == 3) scenario3();
}
}
// CUSTOM FUNCTIONS
void scenario1() {
// add code to perform task scenario 1
tone(speaker, 500);
delay(500); //0.5s
noTone(speaker);
delay(10000); //10s
tone(speaker, 500);
// at end of this task, reset for next task
scenario = 2;
started = false;
}
void scenario2() {
// add code to perform task scenario 2
noTone(speaker);
delay(10000); //10s
tone(speaker, 500);
// at end of this task, reset for next task
scenario = 3;
started = false;
}
void scenario3() {
// add code to perform task scenario 3
unsigned long time = millis(); // get current time in milliseconds
unsigned long endTime = time + 600000; // make end time 10 min from now
// while current time is less than end time, run demo
while (time < endTime) {
avoidCollision();
checkDropOff();
time = millis(); // check current time again
}
// time's up
motors.stop();
alertSound();
// at end of this task, reset for next task
scenario = 1;
started = false;
}
//------------------------------------------------------------------------------------------------
void checkDropOff() {
// IR threshold indicating surface drop-off (table edge, hole, etc.)
int dropOff = 950; // adjust value if necessary
// get IR sensor readings
int leftSensor = leftLine.read();
int centerSensor = centerLine.read();
int rightSensor = rightLine.read();
// see if any IR sensors detect drop-off
if (leftSensor > dropOff || centerSensor > dropOff || rightSensor > dropOff) {
// add code to perform (brake, reverse, change direction, etc.)
motors.brake();
tone(speaker, 1000, 200);
delay(1000);
motors.drive(-150); // back up
delay(1000);
motors.brake();
started = false; // allow test to be repeated
}
}
void checkButton() {
if (button.read() == true) {
// reverse value of "started"
started = !started;
// single-blink and single-beep as feedback
digitalWrite(led, HIGH);
tone(buzzer, 3000);
delay(200);
digitalWrite(led, LOW);
noTone(buzzer);
}
}
void alertSound() {
// modify code to play whatever sound pattern you want
for (int i = 0; i < 3; i++) {
tone(buzzer, 4000);
delay(100);
noTone(buzzer);
delay(100); // pause before next beep
}
}
void driveDistance(float distance, int power) {
// use wheel encoders to drive straight for specified distance at specified power
// set initial power for left and right motors
int leftPower = power;
int rightPower = power;
// amount to offset motor powers to drive straight
int offset = 5;
// if negative distance, make powers & offset also negative
if (distance < 0) {
leftPower *= -1;
rightPower *= -1;
offset *= -1;
}
// adjust distance to improve accuracy
float correction = -1.0; // change value based on test results
if (distance > 0) distance += correction;
else if (distance < 0) distance -= correction;
// variables for tracking wheel encoder counts
long leftCount = 0;
long rightCount = 0;
long prevLeftCount = 0;
long prevRightCount = 0;
long leftDiff, rightDiff;
// RedBot values based on encoders, motors & wheels
float countsPerRev = 192.0; // 192 encoder ticks per wheel revolution
float wheelDiam = 2.56; // wheel diameter = 65 mm = 2.56 in
float wheelCirc = PI * wheelDiam; // wheel circumference = 3.14 x 2.56 in = 8.04 in
// based on distance, calculate number of wheel revolutions
float numRev = distance / wheelCirc;
// calculate target encoder count
float targetCount = numRev * countsPerRev;
// reset encoder counters and start driving
encoder.clearEnc(BOTH);
delay(100);
motors.leftDrive(leftPower);
motors.rightDrive(rightPower);
// keeps looping while right encoder count less than target count
while (abs(rightCount) < abs(targetCount)) {
// get current wheel encoder counts
leftCount = encoder.getTicks(LEFT);
rightCount = encoder.getTicks(RIGHT);
// calculate increase in count from previous reading
leftDiff = abs(leftCount - prevLeftCount);
rightDiff = abs(rightCount - prevRightCount);
// store current counts as "previous" counts for next reading
prevLeftCount = leftCount;
prevRightCount = rightCount;
// adjust left & right motor powers to keep counts similar (drive straight)
// if left rotated more than right, slow down left & speed up right
if (leftDiff > rightDiff) {
leftPower = leftPower - offset;
rightPower = rightPower + offset;
}
// else if right rotated more than left, speed up left & slow down right
else if (leftDiff < rightDiff) {
leftPower = leftPower + offset;
rightPower = rightPower - offset;
}
// apply adjusted motor powers
motors.leftDrive(leftPower);
motors.rightDrive(rightPower);
delay(10); // short delay before next reading
}
// target count reached
motors.brake(); // or use: motors.stop()
delay(500); // brief delay to wait for complete stop
}
void pivotAngle(float angle) {
// use wheel encoders to pivot (turn) by specified angle
// set motor power for pivoting
int power = 100; // turn CW
if (angle < 0) power *= -1; // use negative power to turn CCW
// adjust angle to improve accuracy
float correction = -5.0; // change value based on test results
if (angle > 0) angle += correction;
else if (angle < 0) angle -= correction;
// variable for tracking wheel encoder counts
long rightCount = 0;
// RedBot values based on encoders, motors & wheels
float countsPerRev = 192.0; // 192 encoder ticks per wheel revolution
float wheelDiam = 2.56; // wheel diameter = 65 mm = 2.56 in
float wheelCirc = PI * wheelDiam; // wheel circumference = 3.14 x 2.56 in = 8.04 in
float pivotDiam = 6.125; // pivot diameter = distance between centers of wheel treads = 6.125 in
float pivotCirc = PI * pivotDiam; // pivot circumference = 3.14 x 6.125 in = 19.23 in
// based on angle, calculate distance (arc length)
float distance = abs(angle) / 360 * pivotCirc;
// based on distance, calculate number of wheel revolutions
float numRev = distance / wheelCirc;
// calculate target encoder count
float targetCount = numRev * countsPerRev;
// reset encoder counters and start pivoting
encoder.clearEnc(BOTH);
delay(100);
motors.pivot(power);
// keeps looping while right encoder count less than target count
while (abs(rightCount) < abs(targetCount)) {
// get current wheel encoder count
rightCount = encoder.getTicks(RIGHT);
delay(10); // short delay before next reading
}
// target count reached
motors.brake();
delay(250);
// clearEncoders(); // only needed if using driveStraight() or countLine()
}
void avoidLine() {
/* AVOID LINE
To avoid dark line on light surface:
Use high threshold & see if sensors greater than threshold
To avoid light line on dark surface:
Use low threshold & see if sensors less than threshold
Use test readings from line to determine best value for threshold
*/
// adjust value if necessary
int lineThreshold = 800;
// get IR sensor readings
int leftSensor = leftLine.read();
int rightSensor = rightLine.read();
// when either sensor on line, first brake motors
if (leftSensor > lineThreshold || rightSensor > lineThreshold) {
motors.brake();
delay(250);
}
// when both sensors on line, turn around
if (leftSensor > lineThreshold && rightSensor > lineThreshold) {
long rnd = random(750, 1250);
motors.pivot(100);
delay(rnd);
motors.stop();
}
// when line under left sensor only, pivot right
else if (leftSensor > lineThreshold) {
long rnd = random(500, 750);
motors.pivot(100);
delay(rnd);
motors.stop();
}
// when line under right sensor only, pivot left
else if (rightSensor > lineThreshold) {
long rnd = random(500, 750);
motors.pivot(-100);
delay(rnd);
motors.stop();
}
delay(25); // change delay to adjust line detection sensitivity
}
void avoidCollision() {
// set minimum distance between RedBot and obstacle
float minDist = 8.0; // change value as necessary (need decimal)
// measure distance to nearest obstacle
float sensorDist = measureDistance();
// if obstacle is too close, avoid collision
if (sensorDist <= minDist) {
// add code to perform (brake, change direction, etc.)
motors.brake();
pivotAngle(30);
}
else{
motors.drive(200);
}
delay(60); // need min 60ms between ultrasonic sensor readings
}
float measureDistance() {
// declare local variables for function
unsigned long time1;
unsigned long time2;
unsigned long pulse_time;
float dist_cm;
float dist_in;
// trigger ultrasonic signal for minimum 10 microseconds
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
// wait for echo to be received
while (digitalRead(ECHO_PIN) == 0);
// now measure how long echo lasts (pulse time)
time1 = micros(); // get start time in microseconds
while (digitalRead(ECHO_PIN) == 1); // wait until echo pulse ends
time2 = micros(); // get end time
pulse_time = time2 - time1; // subtract to get amount of time
// pulse time of 23200 represents maximum range for sensor
if (pulse_time > 23200) pulse_time = 23200;
/*
CALCULATE DISTANCE TO OBJECT USING PULSE TIME
distance = pulse time * speed of sound / 2
Divide by 2 because pulse time includes signal traveling to object and then back
Speed of sound in air at sea level is approximately 340 m/s
Numbers below are calculated constants for cm or inches
Decimal point in numbers necessary for accurate float calculation
*/
dist_cm = pulse_time / 58.0;
dist_in = pulse_time / 148.0;
// return measurement (dist_cm or dist_in)
return dist_in;
}

Creating random pixeled lines in Proccesing

I'm trying to make a game and I'm stuck on random level design. Basically, I'm trying to create a line from one edge/corner to another edge/corner while having some randomness to it.
See below image 1 [link broken] and 2 for examples. I'm doing this in processing and every attempt I've tried hasn't yielded proper results. I can get them to populate randomly but not in a line or from edge to edge. I'm trying to do this on a 16 x 16 grid by the way. Any ideas or help would be greatly appreciated thanks!
Image 2:
Based on your description, the challenge is in having a connected line from top to bottom with a bit of randomness driving left/right direction.
There are multiple options.
Here's a basic idea that comes to mind:
pick a starting x position: left's say right down the middle
for each row from 0 to 15 (for 16 px level)
pick a random between 3 numbers:
if it's the 1st go left (x decrements)
if it's the 2nd go right (x increments)
if it's the 3rd: ignore: it means the line will go straight down for this iteration
Here's a basic sketch that illustrates this using PImage to visualise the data:
void setup(){
size(160, 160);
noSmooth();
int levelSize = 16;
PImage level = createImage(levelSize, levelSize, RGB);
level.loadPixels();
java.util.Arrays.fill(level.pixels, color(255));
int x = levelSize / 2;
for(int y = 0 ; y < levelSize; y++){
int randomDirection = (int)random(3);
if(randomDirection == 1) x--;
if(randomDirection == 2) x++;
// if randomDirection is 0 ignore as we don't change x -> just go down
// constrain to valid pixel
x = constrain(x, 0, levelSize - 1);
// render dot
level.pixels[x + y * levelSize] = color(0);
}
level.updatePixels();
// render result;
image(level, 0, 0, width, height);
fill(127);
text("click to reset", 10, 15);
}
// hacky reset
void draw(){}
void mousePressed(){
setup();
}
The logic is be pretty plain above, but free to replace random(3) with other options (perhaps throwing dice to determine direction or exploring other psuedo-random number generators (PRNGs) such as randomGaussian(), noise() (and related functions), etc.)
Here's a p5.js version of the above:
let levelSize = 16;
let numBlocks = levelSize * levelSize;
let level = new Array(numBlocks);
function setup() {
createCanvas(320, 320);
level.fill(0);
let x = floor(levelSize / 2);
for(let y = 0 ; y < levelSize; y++){
let randomDirection = floor(random(3));
if(randomDirection === 1) x--;
if(randomDirection === 2) x++;
// if randomDirection is 0 ignore as we don't change x -> just go down
// constrain to valid pixel
x = constrain(x, 0, levelSize - 1);
// render dot
level[x + y * levelSize] = 1;
}
// optional: print to console
// prettyPrintLevel(level, levelSize, numBlocks);
}
function draw() {
background(255);
// visualise
for(let i = 0 ; i < numBlocks; i++){
let x = i % levelSize;
let y = floor(i / levelSize);
fill(level[i] == 1 ? color(0) : color(255));
rect(x * 20, y * 20, 20, 20);
}
}
function prettyPrintLevel(level, levelSize, numBlocks){
for(let i = 0; i < numBlocks; i+= levelSize){
print(level.slice(i, i + levelSize));
}
}
function mousePressed(){
setup();
}
<script src="https://cdnjs.cloudflare.com/ajax/libs/p5.js/1.4.1/p5.min.js"></script>
The data is a structured a 1D array in both examples, however, if it makes it easier it could easily be a 2D array. At this stage of development, whatever is the simplest, most readable option is the way to go.

calculating wind speed from frequency value

I have NRG #40 wind speed sensor the output frequency is linear with wind speed
output signal range from 0 Hz to 125 Hz
0 Hz mean =0.35 m/s and 125 Hz =96 m/s and transfer function is
m/s = (Hz x 0.765) + 0.35
How can I interface this sensor with a Arduino mega
previously I connect Adafruit (product ID: 1733) which is output voltage not frequency is linear with wind speed
and this code for Adafruit :
//Setup Variables
const int sensorPin = A0; //Defines the pin that the anemometer output is connected to
int sensorValue = 0; //Variable stores the value direct from the analog pin
float sensorVoltage = 0; //Variable that stores the voltage (in Volts) from the anemometer being sent to the analog pin
float windSpeed = 0; // Wind speed in meters per second (m/s)
float voltageConversionConstant = .004882814; //This constant maps the value provided from the analog read function, which ranges from 0 to 1023, to actual voltage, which ranges from 0V to 5V
int sensorDelay = 1000; //Delay between sensor readings, measured in milliseconds (ms)
//Anemometer Technical Variables
//The following variables correspond to the anemometer sold by Adafruit, but could be modified to fit other anemometers.
float voltageMin = .4; // Mininum output voltage from anemometer in mV.
float windSpeedMin = 0; // Wind speed in meters/sec corresponding to minimum voltage
float voltageMax = 2.0; // Maximum output voltage from anemometer in mV.
float windSpeedMax = 32; // Wind speed in meters/sec corresponding to maximum voltage
void setup()
{
Serial.begin(9600); //Start the serial connection
}
void loop()
{
sensorValue = analogRead(sensorPin); //Get a value between 0 and 1023 from the analog pin connected to the anemometer
sensorVoltage = sensorValue * voltageConversionConstant; //Convert sensor value to actual voltage
//Convert voltage value to wind speed using range of max and min voltages and wind speed for the anemometer
if (sensorVoltage <= voltageMin){
windSpeed = 0; //Check if voltage is below minimum value. If so, set wind speed to zero.
}else {
windSpeed = (sensorVoltage - voltageMin)*windSpeedMax/(voltageMax - voltageMin); //For voltages above minimum value, use the linear relationship to calculate wind speed.
}
//Print voltage and windspeed to serial
Serial.print("Voltage: ");
Serial.print(sensorVoltage);
Serial.print("\t");
Serial.print("Wind speed: ");
Serial.println(windSpeed);
delay(sensorDelay);
}
Asuming you use a Arduino UNO or Nano, a easy way is to connect the sensor to pin D2 or D3, witch can be used as Interrupt pins.
You then make a function, or a ISR, that gets called every time the sensor pulses. Then you attach the newly created function to the Interrupt pin.
So it will look something like this.
byte sensorPin = 2;
double pulses = 0;
double wSpeed = 0;
long updateTimer = 0;
int updateDuration = 3000;
void setup() {
Serial.begin(115200);
pinMode(sensorPin, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(sensorPin), sensorISR, FALLING);
}
void loop() {
long now = millis();
if(updateTimer < now) {
updateTimer = now + updateDuration;
wSpeed = ((pulses/(updateDuration/1000)) * 0.765) + 0.35;
pulses = 0;
Serial.println("Windspeed is:" + String(wSpeed));
}
}
void sensorISR() {
pulses++;
}
The ISR functions only job is to increment the pulses variable for every pulse. Then every second you can calculate the frequency and speed. If you wait 3 second instead, like above, you will have a better resolution but will have to account for the extra time in the equation.
I have not testet this code.

Need help reversing the values somehow

I have a very strange problem. Everything in my code works super fine, but I still have an issue.
import processing.serial.*;
float r_height;
float r_width;
float hypotnuse;
int d = 20;
float x ;
float y ;
float ledGlow;
Serial myPort; // Create object from Serial class
void setup () {
size (510, 510);
String portName = Serial.list()[8];
myPort = new Serial(this, portName, 9600);
background (0);
fill(204);
ellipseMode (CORNER);
x = 0;
y = 0;
ellipse (x, y, d, d);
}
void draw () {
r_height = mouseY - y;
r_width = mouseX - x;
println ("Height is " + r_height);
println ("Width is " + r_width);
hypotnuse = sqrt (( (sq(r_height)) + (sq (r_width)) ) );
ledGlow = round (hypotnuse/2.84);
myPort.write(ledGlow);
println (ledGlow);
}
I need to get values 0-255 from the hypotenuse of my triangle. but when I am writing it to the serial port (myPort.write (ledGlow)), i need those values to be flipped. So if the hypotenuse is 0, it actually needs to equal to 255, if it's 1 it needs to be 254 and so on. I am not sure how to approach this problem.
Can you just subtract the hypotenuse from 255 at that time, to get the flipped hypotenuse value?
new_hypotenuse = 255 - hypotenuse ;

Which is best simple Gaussian blur or FFT of Gaussian blur for sigma=20?

I'm making a program to blur a 16 bit grayscale image in CUDA.
In my program, if I use a Gaussian blur function with sigma = 20 or 30, it takes a lot of time, while it is fast with sigma = 2.0 or 3.0.
I've read in some web site that Guaussian blur with FFT is good for large kernel size or large sigma value:
Is It really true ?
Which algorithm should I use: simple Gaussian blur or Gaussian blur with FFT ?
My code for Guassian Blur is below. In my code , is there something wrong or not ?
enter code here
__global__
void gaussian_blur(
unsigned short* const blurredChannel, // return value: blurred channel (either red, green, or blue)
const unsigned short* const inputChannel, // red, green, or blue channel from the original image
int rows,
int cols,
const float* const filterWeight, // gaussian filter weights. The weights look like a bell shape.
int filterWidth // number of pixels in x and y directions for calculating average blurring
)
{
int r = blockIdx.y * blockDim.y + threadIdx.y; // current row
int c = blockIdx.x * blockDim.x + threadIdx.x; // current column
if ((r >= rows) || (c >= cols))
{
return;
}
int half = filterWidth / 2;
float blur = 0.f; // will contained blurred value
int width = cols - 1;
int height = rows - 1;
for (int i = -half; i <= half; ++i) // rows
{
for (int j = -half; j <= half; ++j) // columns
{
// Clamp filter to the image border
int h = min(max(r + i, 0), height);
int w = min(max(c + j, 0), width);
// Blur is a product of current pixel value and weight of that pixel.
// Remember that sum of all weights equals to 1, so we are averaging sum of all pixels by their weight.
int idx = w + cols * h; // current pixel index
float pixel = static_cast<float>(inputChannel[idx]);
idx = (i + half) * filterWidth + j + half;
float weight = filterWeight[idx];
blur += pixel * weight;
}
}
blurredChannel[c + r * cols] = static_cast<unsigned short>(blur);
}
void createFilter(float *gKernel,double sigma,int radius)
{
double r, s = 2.0 * sigma * sigma;
// sum is for normalization
double sum = 0.0;
// generate 9*9 kernel
int m=0;
for (int x = -radius; x <= radius; x++)
{
for(int y = -radius; y <= radius; y++)
{
r = std::sqrtf(x*x + y*y);
gKernel[m] = (exp(-(r*r)/s))/(3.14 * s);
sum += gKernel[m];
m++;
}
}
m=0;
// normalize the Kernel
for(int i = 0; i < (radius*2 +1); ++i)
for(int j = 0; j < (radius*2 +1); ++j)
gKernel[m++] /= sum;
}
int main()
{
cudaError_t cudaStatus;
const int size =81;
float gKernel[size];
float *dev_p=0;
cudaStatus = cudaMalloc((void**)&dev_p, size * sizeof(float));
if (cudaStatus != cudaSuccess) {
fprintf(stderr, "cudaMemcpy failed!");
}
createFilter(gKernel,20.0,4);
cudaStatus = cudaMemcpy(dev_p, gKernel, size* sizeof(float), cudaMemcpyHostToDevice);
if (cudaStatus != cudaSuccess) {
fprintf(stderr, "cudaMemcpy failed!");
}
/* i read image Buffere in unsigned short that code is not added here ,becouse it is large , and copy image data of buffere from host to device*/
/* So, suppose i have unsigned short *d_img which contain image data */
cudaMalloc( (void**)&d_img, length* sizeof(unsigned short));
cudaMalloc( (void**)&d_blur_img, length* sizeof(unsigned short));
static const int BLOCK_WIDTH = 32;
int image_width=1580.0,image_height=1050.0;
int x = static_cast<int>(ceilf(static_cast<float>(image_width) / BLOCK_WIDTH));
int y = static_cast<int>(ceilf(static_cast<float>((image_height) ) / BLOCK_WIDTH));
const dim3 grid (x, y, 1); // number of blocks
const dim3 block(BLOCK_WIDTH, BLOCK_WIDTH, 1);
gaussian_blur<<<grid,block>>>(d_blur_img,d_img,1050.0,1580.0,dev_p,9.0);
cudaDeviceSynchronize();
/* after bluring image i will copied buffer from Device to Host and free gpu memory */
cudaFree(d_img);
cudaFree(d_blur_img);
cudaFree(dev_p);
return 0;
}
Short answer: both algorithms are good with respect to image blurring, so feel free to pick the best (fastest) one for your use case.
Kernel size and sigma value are directly correlated: the greater the sigma, the larger the kernel (and thus the more operations-per-pixel to get the final result).
If you implemented a naive convolution, then you should try a separable convolution implementation instead; it will reduce the computation time by an order of magnitude already.
Now some more insight: they implement almost the same Gaussian blurring operation. Why almost ? It's because taking the FFT of an image does implicitly periodize it. Hence, at the border of the image, the convolution kernel sees an image that has been wrapped around its edge. This is called circular convolution (because of the wrapping). On the other hand, Gaussian blur implements a simple linear convolution.

Resources