Hello,
I'm having trouble with a bit of code that's started to do my head in so any advice on where I've gone wrong would be much appreciated.
The Project:
A simple drawing machine using 2 steppers to move a pen supported on string connected to pulleys on each stepper.
The Code:
The majority of the code is taken up with calculating the position of the pen using trigonometry to calculate triangles derived from the string lengths of either side (stepper steps are counted and converted to a near enough string length).
The position info is to be used to prevent the pen running off the edge of the 'paper'.
The Problem:
Everything runs nicely EXCEPT that the if...else loop that 'tells' when the edge is reach doesn't operate as expected;
When sidecX1 is less than 10 the if...else should 'kick in' but doesn't.
Actually if I 'manually' tell the code that cX1 is less than 10 (with the commented out line sidecX1 = 0.00; for example) if does work.
However when the code runs normally and calculates the value sidecX1 to 0.00 the code runs straight past the if...else without responding???
I have commented out the remaining if...else loop to make it clear where I suspect the problem lies.
I have included the code below and attached it to this post.
I hope someone might be able to advise where I've gone astray.
Many thanks in advance!
cheers Richard
#include <AccelStepper.h>
#include <Stepper.h>
Stepper rightstepper (48, 8,9,10,11); // 200 step stepper 1.8deg note the pin order is not normal
//8 red, 9 black, 10 green, 11 yellow for R S191 - 8356 Motor
Stepper leftstepper (48, 4,5,6,7); // 4 red, 5 black, 6 green, 7 yellow
int XX = 1;
// board size and positioning variables
float boardwidthmm = 300 ; // size of board in mm
float boardheightmm = 300;
float boardwidth = 0; // scaled size of board in steplengths
float boardheight = 0;
float centreposX = 0;
float centreposY = 0;
float Xdistfromcentre = 0;
float Ydistfromcentre = 0;
float centresideA = 0; // how long the strings should be for the pen to be in the centre
float centresideB = 0; // calculated at start and used to reset
float edgewidth = 10.10;
// measuring variables
float stepsperrevolution = 48; // as per stepper
float radiusofpulley = 19; // radius of pulley in mm
float steplength = 0; // mm of string per step
// STEPPER motor variables
int rpm = 4; // how fast the motor should turn
int numberofstepsright = 0; // number of steps to turn right stepper
int numberofstepsleft = 0; // number of steps to turn left stepper
float predictedsideA = 0;
float predictedsideB = 0;
//trigonometry variables
float sideC = 0; // width of board in steps
float sideA = 0; // length of right string in steps
float sideB = 0; //length of left string in steps
float sideY0 = 0; // Altitude of triangle (coordinate Y)
float sidecX0 = 0; // Right hand triangle base along side C
float sidecX1 = 0; // left hand triangle base along side C
float cosC = 0;
float angleC = 0;
float angleCdegrees = 0;
float sinA = 0;
float angleA = 0;
float angleAdegrees = 0;
float angleB = 0;
float angleBdegrees = 0;
boolean hitedge = false;
void setup()
{
Serial.begin(9600);
steplength = ((2*(PI)*(radiusofpulley)/stepsperrevolution)); // calculate the scale from mm to step length
boardwidth = (boardwidthmm / steplength); // convert boardwidth to steplength scale
boardheight = (boardheightmm / steplength);
sideC = boardwidth;
/*
Serial.print( "Boardwidth = ");
Serial.print( boardwidth );
Serial.print( ", Boardheight = ");
Serial.print( boardheight );
Serial.print(" , side C = ");
Serial.println( sideC);
*/
rightstepper.setSpeed(rpm);
leftstepper.setSpeed(rpm);
setcentre();
}
/////////////////
void loop()
{
numberofstepsright = -15;//int(random(-20,0)); // increment or decrement the string length
numberofstepsleft = -10;//int (random(-20,0));
predictedsideA = (sideA + numberofstepsright); // store the new string lengths to check the new position
predictedsideB = (sideB + numberofstepsleft);
Serial.print( " Random steps Right = ");
Serial.print(numberofstepsright);
Serial.print( " , Random of steps Left = ");
Serial.println(numberofstepsleft);
Serial.print( " Predicted side A = ");
Serial.print(predictedsideA);
Serial.print(" , Predicted side B = ");
Serial.println( predictedsideB);
Serial.println();
delay(1000);
calculateposition();
checkedge();
simplesteppermove();
updatesides();
}
///////////////
void setcentre()
{
Serial.println();
Serial.println ( "+++++ SETTING CENTRE ");
centreposX = (boardwidth/2); // calculate the centre of the board using pythagoras
centreposY = (boardheight/2);
sideA = (sqrt( sq(centreposX) + (sq(centreposY))));
sideB = (sqrt( sq(centreposX) + (sq(centreposY))));
centresideA = (sideA);
centresideB = (sideB);
Serial.println ();
Serial.print(" Centre X = ");
Serial.print(centreposX);
Serial.print(" , Centre Y = ");
Serial.println(centreposY);
Serial.print(" Right Centre length = ");
Serial.print(centresideA);
Serial.print(" , Left Centre length = ");
Serial.println(centresideB);
Serial.print( " side A = ");
Serial.print(sideA);
Serial.print(" , side B = ");
Serial.println(sideB);
Serial.println();
}
//////////////
void calculateposition()
{
Serial.println ( "+++CALCULATING POSITION ++++");
// calculate position using lengths sideA, sideB and sideC all in steplength units
cosC = (((sq(predictedsideA)+sq(predictedsideB))-sq(sideC))/(2*(predictedsideA*predictedsideB))); // cosine rule to find largest angle C
angleC = acos(cosC);
sinA = (predictedsideA*(sin(angleC)/sideC)); // sine rule to find angle A
angleA = asin(sinA);
angleB = (PI - (angleC + angleA)); //sum of angles to find last angle B
sideY0 = (predictedsideA * (sin (angleB))); // USING ALTITUDE, side lengths and angle
sidecX0 = sqrt ((sq(predictedsideA))-(sq(sideY0))); //calculate Lengths of cX0 and cX1 using pythagoras
sidecX1 = sqrt ((sq(predictedsideB))-(sq(sideY0))); //side A is ALWAYS the longest side for cX0 & side B is ALWAYS the longest side for cY0
// sidecX1 = 0.00; // manual cX1 to test if...else
Serial.print( " Side cX0 = ");
Serial.print(sidecX0);
Serial.print( " , side cX1 = ");
Serial.print (sidecX1);
Serial.print( " , side Y0 = ");
Serial.println(sideY0);
Serial.println();
}
///////////
void checkedge()
{
Serial.println( " ...... CHECK EDGE ");
/* if ((predictedsideA + predictedsideB) < boardwidth + 10) // prevent string becoming too tight
{
//hitedge = true;
Serial.println ("too short");
returntocentre();
}
else / if (sidecX1 < 10) // || (sidecX1 >(boardwidth -10 )))// || (sidecX0 < 10)))
{
//hitedge = true;
Serial.println (" hit side");
returntocentre();
}
/ else if ((sideY0 < 10) || (sideY0 > (boardheight - 10)))
{
//hitedge = true;
Serial.println (" hit bottom ");
returntocentre();
}
*/
}
/////////////////////
void returntocentre()
{
Serial.println ( "...RETURN TO CENTRE");
numberofstepsright = int(centresideA - sideA); // string length MINUS centre
numberofstepsleft = int(centresideB - sideB); //string lengths
Serial.print (" Return steps right = ");
Serial.print ( numberofstepsright);
Serial.print (" , Return steps left = ");
Serial.println( numberofstepsleft);
predictedsideA = (sideA + numberofstepsright); // predictedsideA = centresideA;
predictedsideB = (sideB + numberofstepsleft); //predictedsideB = centresideB;
}
/////////
void updatesides()
{
sideA = predictedsideA;
sideB = predictedsideB;
Serial.print(" Side A = ");
Serial.print( sideA);
Serial.print(" , Side B = ");
Serial.println (sideB);
}
/////////
void simplesteppermove()
{
Serial.println( " STEPPER MOVING ");
rightstepper.step(numberofstepsright);
leftstepper.step(numberofstepsleft);
Serial.print ( "Stepper moves right = ");
Serial.print (numberofstepsright);
Serial.print ( " , Stepper moves left = ");
Serial.println(numberofstepsleft);
Serial.println();
}
