Author Topic: Arudino Code help  (Read 4389 times)

pwatson22

  • New Member
  • *
  • Posts: 8
    • View Profile
Arudino Code help
« on: August 06, 2013, 10:42:23 AM »
Hey there,

I'm working on a low-profile solar tracker using a lead screw on both the x- and y-axes, but am encountering some problems with my Arduino code.  I believe the problem is that the zero on my y-axis track is beyond the limits of my track, so in the mornings, the tracker wants to move beyond where it can actually go.  I've compensated for this by using an 'if' loop to have the mount move the same distance but in the opposite direction on the track; this initially solves that problem, but then causes the azimuth angle to be off, whih prevents m program from entering closed loop control, where our photosensor can provide feedback on if the tracker is perfectly normal to the sun. My code is below, any help is greatly appreciated.


Quote
/*
Arduino source code for Helios Solar Tracker.
Written by Laughlin Barker BSME & BS Environmental Studies '12
& Adapted by Patrick Watson
*/
#include <AccelStepper.h>
#include <AFMotor.h>
#include <Wire.h>
#include "RTClib.h"

RTC_DS1307 RTC;      //initialize RTC

//========================================
//==============  CONSTANTS  =============
//========================================
float latitude = 37.3477;      //SCU
float longitude = -121.9513;
//float timezone = -8;            //Timezone

float l1 = 16;        //in - L1 on mirror structure (back arm)
float l2 = 14.69;        //in - L2 on mirror structure

float pitch = 0.125;    //in - leadscrew pitch (3/8 - 12 screw)
float starts = 2;        //# start
float lead = pitch*starts;  //in - lead of screw (distance traveled in 1 rotation)
float cartwidth = 4.5;        //in - width of cart on actuator
float lx = 48;              //in - length of x actuator space
float ly = 33.2;              //in - length of y actuator space

float stepper_resolution = 200;    //resolution of stepper

float xlims[2], ylims[2];    //x and y max/min in form [xmin, xmax], [ymin, ymax]
float args[2] = {180, 0};
long stepsx,stepsy;

float pi = 3.14159265;

//Logical holders for stepper zeroing
boolean xcal = 0;    //0 == FALSE i.e. not zeroed
boolean ycal = 0;
//========================================
//========================================


//========================================
//=============  GLOBAL VARS  ============
//========================================
float azi = 0;
float ele = 0;          //open loop azi and ele
float x, y;             //open loop x and y
//=====================


//================== MOTOR CONFIG STUFF ================
AF_Stepper motor1(200, 1); //X motor
AF_Stepper motor2(200, 2); //Y motor

void forwardstep1() {             
  motor1.onestep(FORWARD, DOUBLE);
}
void backwardstep1() { 
  motor1.onestep(BACKWARD, DOUBLE);
}

void forwardstep2() { 
  motor2.onestep(FORWARD, DOUBLE);   
}
void backwardstep2() { 
  motor2.onestep(BACKWARD, DOUBLE);
}
// Motor shield has two motor ports, now we'll wrap them in an AccelStepper object
//AccelStepper stepper1(forwardstep1, backwardstep1);
AccelStepper stepper1(forwardstep1, backwardstep1);
AccelStepper stepper2(forwardstep2, backwardstep2);

int xcalpin = 19;
int ycalpin = 18;

void setup()

    //setup steppers
    stepper1.setMaxSpeed(300);    //steps/s
    stepper1.setAcceleration(500);
    stepper2.setMaxSpeed(300);
    stepper2.setAcceleration(500);
   
    //start serial
    Serial.begin(115200);
   
    //start 1-wire bus & RTC
    Wire.begin();
    RTC.begin();
   
    //convert to rads
    latitude = latitude * pi/180;
   
    //uncomment if RTC is significantly off, upload, comment out, and then re-reupload
//    RTC.adjust(DateTime(__DATE__, __TIME__));
}

char X_buffer[8];
float maxPOS = 32e3;
float setPointPOS, requestedPOS;
int j=1;

long previousMillis = 0;
long interval = 5000;

void loop() //begin main loop
{
  DateTime now = RTC.now();
  //if X and Y not calibrated, run motors until limit switch hit; will run on every restart
//   Serial.println("Zeroing X Actuator");
   while (xcal == 0){
    stepper2.moveTo(-100000);
    stepper2.run();
    Serial.println("Calibrating X");
     
      if (digitalRead(xcalpin)==0){
        xzero();
      }
     
    }
   while (ycal == 0){
    stepper1.moveTo(-100000);
    stepper1.run();
    Serial.println("Calibrating Y");
     
      if (digitalRead(ycalpin)==0){
        yzero();
        Serial.println("Both actuators properly zeroed");
        Serial.print(stepper2.currentPosition());
        Serial.print(" , ");
        Serial.print(stepper1.currentPosition());
       

      }
     
    }
   
   
    if (tracker_azi() >= 255){
      stepper2.moveTo(-10);
      stepper1.run();
      stepper2.run();
    }
//    if (stepper1.currentPosition() <= 4720){
//      stepper1.moveTo(18450);
//      stepper1.run();
//      stepper2.run();
//    }



  //==========FOR X===========
if (Serial.available()){
      //make sure we have all the data
      delay(5);
      //serial data is waiting, lets extract     
      int  i=0;     
      while(i<8){
        X_buffer = Serial.read();
        //Serial.println(X_buffer);
        i++;
      }
     
      //flush serial buffer of extra data>9999
      Serial.flush();
      //convert numeric string to int var
      requestedPOS = atof(X_buffer);
      //check for set point greater than maxPWR limit
      if( requestedPOS < maxPOS){
        //Buffer contains a safe value, copy out to setPointPWR
        setPointPOS = requestedPOS;
//        Serial.print("Requested set point:");
//        Serial.println(setPointPOS);
      }
     
      else{
        Serial.println("'The sun don't shine here!"); //errorcode 2012, you requested a pathalogical power level
        setPointPOS = 0; //
      }
      if (j>0){
        Serial.print("Accepted AZI position:");
        Serial.println(requestedPOS);
        azi = setPointPOS;
        //stepsx = setPointPOS;
      }
      else if (j<0){
        Serial.print("Accepted ELE position:");
        Serial.println(requestedPOS);
        ele = setPointPOS;
        //stepsy = setPointPOS;
      }
      j=-j;
    }
     

 
 stepper1.run();
 stepper2.run();
 

     unsigned long currentMillis = millis();
 
  if(currentMillis - previousMillis > interval) {
    // save the last time it went through this routine
    previousMillis = currentMillis;   
   
 //================================================================================
 //                PERIODIC CALCULATIONS (not nec. every loop iteration)
 //================================================================================
   
    //get AZI and ELE of sun based on RTC
    float azi_s = azi_func(latitude,longitude);
    float ele_s = ele_func(latitude,longitude);
   
    //instantaneous AZI and ELE of tracker
    float azi_t = tracker_azi();
    float ele_t = tracker_ele();
   
    //tracker commanded position vars
    float azi_t_command;
    float ele_t_command;
    long xcommand;
    long ycommand;
   

   
       azi_t_command = azi_s;
       ele_t_command = ele_s;
       xcommand = x_step_pos(azi_t_command, ele_t_command);
       ycommand = y_step_pos(azi_t_command, ele_t_command);
     
      if (stepper1.currentPosition() == ycommand & stepper2.currentPosition() == xcommand){
        ele_t = ele_s;
        azi_t = azi_s;
      }
     
      float RTCerr = pow( (pow( (azi_s - azi_t) , 2) + pow( (ele_s - ele_t) , 2 ) ) , 0.5);
   
//    float RTCerr = pow(pow( (xcommand-stepper2.currentPosition()),2) + pow( (ycommand-stepper1.currentPosition()),2),0.5);
//    if angular error ir greater than maxRTCerr, move tracker to sun orientation
   
    float maxRTCerr = 3;        //degrees total error (sum of azi/ele error vectors)
     if (RTCerr > maxRTCerr){
       

       
       if (ycommand < 4720){
         stepper1.moveTo(4720 + 1.9 * (4720-ycommand));
         stepper2.moveTo(xcommand);
         stepper1.run();
         stepper2.run();
         ele_t_command = stepper1.currentPosition();
         ycommand = y_step_pos(azi_t_command, ele_t_command);
         ele_t = ele_s;
       }
       else{
       stepper1.moveTo(ycommand);
     }
       
       stepper2.moveTo(xcommand);

//stepper1.moveTo(ycommand);
//stepper2.moveTo(xcommand);
       
       Serial.println("=====================================");
       
       Serial.println("OPEN-LOOP POSITIONING...");
       
       Serial.println("=====================================");
     }
     
     // Closed loop routine; only do it RTC error is within allowable limits
     else if(RTCerr <= maxRTCerr){
       
       float EWcorr = senseEW();
       float NScorr = senseNS();
       
       if (EWcorr != 0 || NScorr != 0){
         azi_t_command = azi_t + EWcorr;
         ele_t_command = ele_t + NScorr;
         
         xcommand = x_step_pos(azi_t_command, ele_t_command);
         ycommand = y_step_pos(azi_t_command, ele_t_command);
       }
       else{
         
       xcommand = x_step_pos(azi_t_command, ele_t_command);
       ycommand = y_step_pos(azi_t_command, ele_t_command);
       }
       
       stepper1.moveTo(ycommand);
       stepper2.moveTo(xcommand);
       
       delay(5000);
       
       Serial.println("=====================================");
       
       Serial.println("CLOSED LOOP MISALIGNMENT...CORRECTING");
       Serial.print("Correcting ");
       Serial.print(EWcorr);
       Serial.println(" degrees West...");

       Serial.print("Correcting ");
       Serial.print(NScorr);
       Serial.println(" degrees North");
       
       Serial.println("=====================================");     
     }
       
 long xcurrent = stepper2.currentPosition();
 long ycurrent = stepper1.currentPosition();

 //================================================================================
 //                PERIODIC SERIAL OUTPUT
 //================================================================================
   
    Serial.println("Step pos: ");
    Serial.print(xcurrent);
    Serial.print(" , ");
    Serial.println(ycurrent);
    Serial.println(" ");
   
//    Serial.print("X,Y: ");
//    Serial.print(steps2disp(xcurrent));
//    Serial.print(" , ");
//    Serial.println(steps2disp(ycurrent));
//    Serial.println(" ");
   
    Serial.print("Date: ");
    Serial.print(now.year(), DEC);
    Serial.print('/');
    Serial.print(now.month(), DEC);
    Serial.print('/');
    Serial.print(now.day(), DEC);
    Serial.print(' ');
    Serial.print(now.hour(), DEC);
    Serial.print(':');
    Serial.print(now.minute(), DEC);
    Serial.print(':');
    Serial.println(now.second(), DEC);
    Serial.println(" ");
   
    Serial.print("Sun Pos: ");
    Serial.print(azi_s);
    Serial.print(" , ");
    Serial.println(ele_s);
    Serial.println(" ");
   
 

    Serial.print("Tracker Pos: ");
    Serial.print(tracker_azi());
    Serial.print(" , ");
    Serial.println(ele_t);
   

   
    Serial.println("========================");

  }
 
}


void xzero(){
  Serial.println("Zeroing X actuator");
  long tmppos = disp2steps(lx/2) - (cartwidth/2) * stepper_resolution / lead;
  tmppos = -tmppos;
   //calculating position based on screw and stepper info
  stepper2.setCurrentPosition(tmppos);    //makes zero in the middle of the x actuator when cart is at end...sneaky!
  Serial.print("Current position: ");
  Serial.println(stepper2.currentPosition());
  stepsx = stepper2.currentPosition();
  xcal = 1;
}

void yzero(){
  Serial.println("Zeroing Y actuator");
   //calculating position based on screw and stepper info
  long tmppos = 2.95 * stepper_resolution / pitch;
  stepper1.setCurrentPosition(tmppos);    //makes zero in the middle of the y actuator when cart is at end...sneaky!
  Serial.print("Current position: ");
  Serial.println(stepper1.currentPosition());
  stepsy = stepper1.currentPosition();
  ycal = 1;
 
}

long disp2steps(float disp){      //actuator displacement to steps
  long steps = disp * stepper_resolution / lead;    //will truncate; OK since 1 step = 0.00125" for .25 lead screw
  return steps;
}

float steps2disp(long isteps){
  float steps = (long)isteps;
  float disp = steps/(stepper_resolution / pitch);
  return disp;
}


float tracker_azi(){
  float x = steps2disp(stepper2.currentPosition());
  float y = steps2disp(stepper1.currentPosition());
  float azi = azi_pos(x, y, l1, l2, args);
  return azi;
}

float tracker_ele(){
  float x = steps2disp(stepper2.currentPosition());
  float y = steps2disp(stepper1.currentPosition());
  float ele = ele_pos(x, y, l1, l2, args);
  return ele;
}

long x_step_pos(float azi, float ele){
  float x = aziele2x(azi, ele, l1, l2, args);
  long x_steps = disp2steps(x);
  return x_steps;
}

long y_step_pos(float azi, float ele){
  float y = aziele2y(azi, ele, l1, l2, args);
  long y_steps = disp2steps(y);
  return y_steps;
}
« Last Edit: August 06, 2013, 11:05:41 AM by pwatson22 »


Gabriel

  • Administrator
  • Hero Member
  • *****
  • Posts: 654
    • View Profile
Re: Arudino Code help
« Reply #1 on: August 08, 2013, 09:35:20 AM »
Hey pwatson22!

I'm not exactly sure where to start on this one? Where did you get the code? Did you write most of it yourself, or did you get it from someone else? It looks like the sun calculations were done by me, but they are actually obsolete. I have since redone them if you are interested. They are now inside a library, so it shouldn't be too difficult to include them. You can find it in the Sun_Tracking_Heliostat_Program folder on this page. The older version you are using might not even work correctly. At the very least it is less accurate. Of course, it sounds like making an inaccurate solar tracker more precise using photosensors is the whole point of your project, so maybe they are perfect for you. :)

Where is the if statement you added? For linear actuators, I really wouldn't expect that arbitrarily telling them to move in the opposite direction would give you what you want. Maybe I misunderstood, but is there any reason that you can't just tell the machine to stop if it tries to move outside of it's physical boundaries?

Also, did you have an example picture of how your machine moves? Even one of someone elses would be fine.
I'm asking because the functions aziele2y and aziele2x seem to be a bit more complicated than I would expect for a typical 2-axes machine with linear actuators. Did you write these specifically for your machine, or did you find them somewhere?

Thanks!
Gabriel


pwatson22

  • New Member
  • *
  • Posts: 8
    • View Profile
Re: Arudino Code help
« Reply #2 on: August 09, 2013, 09:26:49 AM »
Most of this code was written by another student before me, who I believe adapted quite a bit of it from your previous posts. But thank you for letting me know those calculations are inaccurate, I did not realize that. As for the 'if' statement I added, it was the statement:
Code: [Select]
       if (ycommand < 4720){
         stepper1.moveTo(4720 + 1.9 * (4720-ycommand));
         stepper2.moveTo(xcommand);
         stepper1.run();
         stepper2.run();
         ele_t_command = stepper1.currentPosition();
         ycommand = y_step_pos(azi_t_command, ele_t_command);
         ele_t = ele_s;
       }

As for the aziele2y and aziele2x functions, they were in the program when I started working with it. I'm not completely sure as to their origins, I've mostly been working on altering this code to function with our current set-up. 

A picture of how the tracker moves is attached.

Thank you very much for your response.

pwatson22

  • New Member
  • *
  • Posts: 8
    • View Profile
Re: Arudino Code help
« Reply #3 on: August 09, 2013, 03:04:07 PM »
Also, do you have any tips about easily modifying my current code to use the sun position library?

I'm a bit new to Arduino and am still learning how to use libraries/functions and such.

Thank you.

Gabriel

  • Administrator
  • Hero Member
  • *****
  • Posts: 654
    • View Profile
Re: Arudino Code help
« Reply #4 on: August 12, 2013, 09:00:26 AM »
Hi pwatson22,

I've been away for the weekend, so this reply was delayed a bit.

I can't see what's wrong just by looking at the code, but if I had access to the machine what I would do is basically just comment everything I could out of the program except for what is needed to move the machine. So forget about the sensing stuff and forget about the sun_pos_calculations for now. Instead of calculating the altitude and azimuth values, just hard code some values in and see if the machine actually moves to them correctly.

Once you have this working reliably, it should be a piece of cake to add the rest of the code back in.

To include the newer sun calculation library, first just add this line at the top of the code like you do with all libraries.
Code: [Select]
  #include "SunPositionAlgo_LowAc.h"


Then put this code where you need to calculate the sun's altitude and azimuth in your program.
Code: [Select]

float SunsAltitude, SunsAzimuth, h, delta;

      SunPositionAlgo_LowAc::CalculateSunsPositionLowAc(month, day, hour, minute, second, timezone, latitude, longitude, SunsAltitude, SunsAzimuth, delta, h);
      SunsAltitude = SunsAltitude + (1.02/tan((SunsAltitude + 10.3/(SunsAltitude + 5.11)) * pi/180.0))/60.0;//Refraction Compensation: Meeus Pg. 105

           Serial.print("Sun's Alt: ");
           Serial.println(SunsAltitude,3);
           Serial.print("Sun's Az: ");
           Serial.println(SunsAzimuth , 3);


It might not be immediately obvious, but the variables SunsAltitude, SunsAzimuth, h,  and delta are automatically changed to new values after the SunPositionAlgo_LowAc function runs. You won't need h and delta.
You will of course need to add code that sets the correct values for these variables.
month, day, hour, minute, second, timezone, latitude, longitude


pwatson22

  • New Member
  • *
  • Posts: 8
    • View Profile
Re: Arudino Code help
« Reply #5 on: August 21, 2013, 01:19:32 PM »
Thank you so much for your help. Everything seems to be working great right now, although I haven't had a chance to test the tracking in the sun yet. I do have a question though.  When I've been running the program with the new sun positioning program, I've been getting values for the elevation that are larger than what I've found from calculators online. Is this due to solar declination?

I've attached a picture below.  At this point in time, the calculators said that the elevation would be 64.48, compared with the value of 78.24 calculated by the program.

Gabriel

  • Administrator
  • Hero Member
  • *****
  • Posts: 654
    • View Profile
Re: Arudino Code help
« Reply #6 on: August 22, 2013, 07:57:19 AM »
I don't know which calculators you are using, but in general if the two programs are giving different elevations it's probably because they have been differing input. It is something that is very very easy to do because there are so many variables to keep in order. One common mistake is to not compensate for daylight saving time correctly since different programs can deal with it differently. Maybe add or subtract an hour to the online calculator and see if that fixes it.
If you have a smartphone, you can probably find a free app that will calculate the position of the sun. Since they usually have a built in GPS and the time is set automatically, using one can help remove any ambiguity regarding which set of values are the correct ones.

You might also read through this page, particularly the part on double checking to see if the time on the RTC is set correctly.
http://www.cerebralmeltdown.com/setting-the-time-on-the-real-time-clock/

You can also expect the output to be different depending on the calculator. Some people prefer the elevation to be measured from the zenith, while others prefer the horizon.

I can't tell you how many times I've been confused when using other sun position calculators, so you're not alone at least.

pwatson22

  • New Member
  • *
  • Posts: 8
    • View Profile
Re: Arudino Code help
« Reply #7 on: August 22, 2013, 01:15:12 PM »
Yeah, some of these things can be pretty confusing.  But I wet outside and tested my tracker today and found that it is going to an elevation that is too high. The elevation appears to be correct at the earlier hours in the day, but when the sun is approaching it's peak, it appears to go too far. My test from today was about the same time as the test I ran yesterday, so the peak is still approximately 78 degrees, whereas the info I got from http://www.sunposition.info/sunposition/spc/locations.php#1 indicate that the max should be somewhere around 64.  Unfortunately my closed loop positioning is still not correct so my sensor can't fix it, so can you think of anyway to alter the elevation without the use of the sensor?

My sensor also is indicating that my position is off, but instead of correcting, I'm just hearing the motors click. Does this sound like my correction isn't big enough or that my motors aren't getting the correct command?  Any thoughts are welcome.

Thank you

pwatson22

  • New Member
  • *
  • Posts: 8
    • View Profile
Re: Arudino Code help
« Reply #8 on: August 22, 2013, 01:17:54 PM »
I meant to attach these to the last reply. The spreadsheet displays the different positions of the sun from true north on 8/22/13 from San Jose, CA (about 10 miles from me). And the picture is the graph of the position over time for the same day.

Gabriel

  • Administrator
  • Hero Member
  • *****
  • Posts: 654
    • View Profile
Re: Arudino Code help
« Reply #9 on: August 23, 2013, 07:21:47 PM »
If the elevation is still so far off, there must be something off in the settings / code.

Did you ever remove that line of code in setup() function that turns the latitude from degrees to radians? You don't need this if you are using the library.

You can try posting your updated version if that's not it, and I'll double check some things.

I'm not sure what's with the stepper motors. It's pretty much impossible to tell without being in the same room as them to debug. The clicking could be from it only making very small moves, but like I said I can't really tell from here.




pwatson22

  • New Member
  • *
  • Posts: 8
    • View Profile
Re: Arudino Code help
« Reply #10 on: August 29, 2013, 10:36:38 AM »
That's what it was. Thank you very much.

Is it possible to input a string to the Serial Monitor that can initialize part of the program?
I was thinking of trying to allow the user to input a word like "stop" that will cause the tracker to return to some set position and shut down.

Any ideas are welcome.

Thanks

Gabriel

  • Administrator
  • Hero Member
  • *****
  • Posts: 654
    • View Profile
Re: Arudino Code help
« Reply #11 on: August 29, 2013, 06:31:49 PM »
Glad it's working!

I'm certain that it is possible to send commands through the Serial Monitor to initialize different parts of the program. The closest example I have is the one on this page.

I don't know if it will do exactly what you want though since it stops the entire program while it waits for your input. I guess you could get around that though just by making the getFloatFromSerialMonitor function terminate its while loop after a certain amount of time has passed (i.e. set x=2 after say 5 seconds have passed). That would be a quick and dirty method I guess.

pwatson22

  • New Member
  • *
  • Posts: 8
    • View Profile
Re: Arudino Code help
« Reply #12 on: September 12, 2013, 03:26:09 PM »
I've gotta say thank you for all of your help. Our tracker is working better than ever now thanks to your input.

I'm attempting to add in a joystick for manual control like you have in your program, but am trying to integrate it into the current program I am using. 
However, you have quite a few functions and such defined that we do not have in our code. Will just adding the "Joystick_Control_Code" and pieces from the actual program allow this to work? Or are there things defined in several layers that I will need to implement?

Thank you for all of your help

Gabriel

  • Administrator
  • Hero Member
  • *****
  • Posts: 654
    • View Profile
Re: Arudino Code help
« Reply #13 on: September 17, 2013, 07:15:04 AM »
Great! Glad it's working!

You've probably already figured it out by now, but the joystick code isn't quite copy and paste.
There are several global variables and even global arrays. Since you are only using one machine, then it probably isn't necessary for them to be global.

As I remember it though, it didn't really take all that long to put that code together. It might be less confusing to just start from scratch since some of that stuff isn't required for you machine whatsoever.

Sorry about the delay. It's been a busy week!