Non Optical Solar Tracker (East Tower 2.4KW)

1// ---------------------------------------------------------------------------
2//  Created by Francisco Malpartida on 20/08/11.
3// Copyright 2011 - Under creative  commons license 3.0:
4//        Attribution-ShareAlike CC BY-SA
5//
6// This  software is furnished "as is", without technical support, and with no 
7// warranty,  express or implied, as to its usefulness for any purpose.
8//
9// Thread Safe:  No
10// Extendable: Yes
11//
12// @file LiquidCrystal_I2C.c
13// This file implements  a basic liquid crystal library that comes as standard
14// in the Arduino SDK but  using an I2C IO extension board.
15// 
16// @brief 
17// This is a basic implementation  of the LiquidCrystal library of the
18// Arduino SDK. The original library has  been reworked in such a way that 
19// this class implements the all methods to  command an LCD based
20// on the Hitachi HD44780 and compatible chipsets using  I2C extension
21// backpacks such as the I2CLCDextraIO with the PCF8574* I2C IO  Expander ASIC.
22//
23// The functionality provided by this class and its base  class is identical
24// to the original functionality of the Arduino LiquidCrystal  library.
25//
26//
27//
28// @author F. Malpartida - fmalpartida@gmail.com
29//  ---------------------------------------------------------------------------
30#if  (ARDUINO <  100)
31#include <WProgram.h>
32#else
33#include <Arduino.h>
34#endif
35#include  <inttypes.h>
36#include "I2CIO.h"
37#include "LiquidCrystal_I2C.h"
38
39//  CONSTANT  definitions
40// ---------------------------------------------------------------------------
41
42//  flags for backlight control
43/*!
44 @defined 
45 @abstract   LCD_NOBACKLIGHT
46  @discussion NO BACKLIGHT MASK
47 */
48#define LCD_NOBACKLIGHT 0x00
49
50/*!
51  @defined 
52 @abstract   LCD_BACKLIGHT
53 @discussion BACKLIGHT MASK used when  backlight is on
54 */
55#define LCD_BACKLIGHT   0xFF
56
57
58// Default library  configuration parameters used by class constructor with
59// only the I2C address  field.
60// ---------------------------------------------------------------------------
61/*!
62  @defined 
63 @abstract   Enable bit of the LCD
64 @discussion Defines the IO of  the expander connected to the LCD Enable
65 */
66#define EN 2  // Enable bit
67
68/*!
69  @defined 
70 @abstract   Read/Write bit of the LCD
71 @discussion Defines the  IO of the expander connected to the LCD Rw pin
72 */
73#define RW 1  // Read/Write  bit
74
75/*!
76 @defined 
77 @abstract   Register bit of the LCD
78 @discussion  Defines the IO of the expander connected to the LCD Register select pin
79 */
80#define  RS 0  // Register select bit
81
82/*!
83 @defined 
84 @abstract   LCD dataline  allocation this library only supports 4 bit LCD control
85 mode.
86 @discussion  D4, D5, D6, D7 LCD data lines pin mapping of the extender module
87 */
88#define  D4 4
89#define D5 5
90#define D6 6
91#define D7 7
92
93
94// CONSTRUCTORS
95//  ---------------------------------------------------------------------------
96LiquidCrystal_I2C::LiquidCrystal_I2C(  uint8_t lcd_Addr )
97{
98   config(lcd_Addr, EN, RW, RS, D4, D5, D6, D7);
99}
100
101
102LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t  lcd_Addr, uint8_t backlighPin, 
103                                     t_backlighPol  pol = POSITIVE)
104{
105   config(lcd_Addr, EN, RW, RS, D4, D5, D6, D7);
106   setBacklightPin(backlighPin,  pol);
107}
108
109LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t lcd_Addr, uint8_t  En, uint8_t Rw,
110                                     uint8_t Rs)
111{
112   config(lcd_Addr,  En, Rw, Rs, D4, D5, D6, D7);
113}
114
115LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t  lcd_Addr, uint8_t En, uint8_t Rw,
116                                     uint8_t  Rs, uint8_t backlighPin, 
117                                     t_backlighPol  pol = POSITIVE)
118{
119   config(lcd_Addr, En, Rw, Rs, D4, D5, D6, D7);
120   setBacklightPin(backlighPin,  pol);
121}
122
123LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t lcd_Addr, uint8_t  En, uint8_t Rw,
124                                     uint8_t Rs, uint8_t d4,  uint8_t d5,
125                                     uint8_t d6, uint8_t d7 )
126{
127   config(lcd_Addr, En, Rw, Rs, d4, d5, d6, d7);
128}
129
130LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t  lcd_Addr, uint8_t En, uint8_t Rw,
131                                     uint8_t  Rs, uint8_t d4, uint8_t d5,
132                                     uint8_t d6,  uint8_t d7, uint8_t backlighPin, 
133                                     t_backlighPol  pol = POSITIVE )
134{
135   config(lcd_Addr, En, Rw, Rs, d4, d5, d6, d7);
136   setBacklightPin(backlighPin,  pol);
137}
138
139// PUBLIC METHODS
140// ---------------------------------------------------------------------------
141
142//
143//  begin
144void LiquidCrystal_I2C::begin(uint8_t cols, uint8_t lines, uint8_t dotsize)  
145{
146   
147   init();     // Initialise the I2C expander interface
148   LCD::begin  ( cols, lines, dotsize );   
149}
150
151
152// User commands - users can expand  this section
153//----------------------------------------------------------------------------
154//  Turn the (optional) backlight off/on
155
156//
157// setBacklightPin
158void LiquidCrystal_I2C::setBacklightPin  ( uint8_t value, t_backlighPol pol = POSITIVE )
159{
160   _backlightPinMask = (  1 << value );
161   _polarity = pol;
162   setBacklight(BACKLIGHT_OFF);
163}
164
165//
166//  setBacklight
167void LiquidCrystal_I2C::setBacklight( uint8_t value ) 
168{
169   // Check if backlight is available
170   // ----------------------------------------------------
171   if ( _backlightPinMask != 0x0 )
172   {
173      // Check for polarity to configure  mask accordingly
174      // ----------------------------------------------------------
175      if  (((_polarity == POSITIVE) && (value > 0)) || 
176           ((_polarity  == NEGATIVE ) && ( value == 0 )))
177      {
178         _backlightStsMask = _backlightPinMask  & LCD_BACKLIGHT;
179      }
180      else 
181      {
182         _backlightStsMask  = _backlightPinMask & LCD_NOBACKLIGHT;
183      }
184      _i2cio.write( _backlightStsMask  );
185   }
186}
187
188
189// PRIVATE METHODS
190// ---------------------------------------------------------------------------
191
192//
193//  init
194int LiquidCrystal_I2C::init()
195{
196   int status = 0;
197   
198   //  initialize the backpack IO expander
199   // and display functions.
200   // ------------------------------------------------------------------------
201   if ( _i2cio.begin ( _Addr ) == 1 )
202   {
203      _i2cio.portMode ( OUTPUT  );  // Set the entire IO extender to OUTPUT
204      _displayfunction = LCD_4BITMODE  | LCD_1LINE | LCD_5x8DOTS;
205      status = 1;
206      _i2cio.write(0);  // Set  the entire port to LOW
207   }
208   return ( status );
209}
210
211//
212// config
213void  LiquidCrystal_I2C::config (uint8_t lcd_Addr, uint8_t En, uint8_t Rw, uint8_t Rs,  
214                                uint8_t d4, uint8_t d5, uint8_t d6, uint8_t  d7 )
215{
216   _Addr = lcd_Addr;
217   
218   _backlightPinMask = 0x08;
219   _backlightStsMask  = LCD_BACKLIGHT;
220   _polarity = POSITIVE;
221   
222   _En = ( 1 << En );
223   _Rw = ( 1 << Rw );
224   _Rs = ( 1 << Rs );
225   
226   // Initialise pin mapping
227   _data_pins[0] = ( 1 << d4 );
228   _data_pins[1] = ( 1 << d5 );
229   _data_pins[2]  = ( 1 << d6 );
230   _data_pins[3] = ( 1 << d7 );   
231}
232
233
234
235// low  level data pushing commands
236//----------------------------------------------------------------------------
237
238//
239//  send - write either command or data
240void LiquidCrystal_I2C::send(uint8_t value,  uint8_t mode) 
241{
242   // No need to use the delay routines since the time taken  to write takes
243   // longer that what is needed both for toggling and enable  pin an to execute
244   // the command.
245   
246   if ( mode == FOUR_BITS )
247   {
248      write4bits( (value & 0x0F), COMMAND );
249   }
250   else 
251   {
252      write4bits( (value >> 4), mode );
253      write4bits( (value & 0x0F), mode);
254   }
255   delay(1);	
256}
257
258//
259// write4bits
260void LiquidCrystal_I2C::write4bits  ( uint8_t value, uint8_t mode ) 
261{
262   uint8_t pinMapValue = 0;
263   
264   // Map the value to LCD pin mapping
265   // --------------------------------
266   for ( uint8_t i = 0; i < 4; i++ )
267   {
268      if ( ( value & 0x1 ) == 1  )
269      {
270         pinMapValue |= _data_pins[i];
271      }
272      value  = ( value >> 1 );
273   }
274   
275   // Is it a command or data
276   // -----------------------
277   if ( mode == DATA )
278   {
279      mode = _Rs;
280   }
281   
282   pinMapValue  |= mode | _backlightStsMask;
283   pulseEnable ( pinMapValue );
284}
285
286//
287//  pulseEnable
288void LiquidCrystal_I2C::pulseEnable (uint8_t data)
289{
290   _i2cio.write  (data | _En);   // En HIGH
291   _i2cio.write (data & ~_En);  // En LOW
292}

1//#include <SFE_BMP180.h>
2#include <avr/wdt.h>
3#include <Wire.h>
4#include   <LSM303.h>       // modified ... fixed a couple of bugs
5#include <LiquidCrystal_I2C.h>
6//#include   <L3G.h>
7#include "ds3231.h"
8#include "ht16k33.h"
9#include <ModbusRtu.h>    // Modified ... this no longer a stock lib - Slave supports register translation/mapping
10#include   <EEPROM.h>
11#include <math.h>
12
13#define ID   1
14
15#define BUFF_MAX 32
16#define   PARK_EAST 1
17#define PARK_WEST 2
18#define PARK_NORTH 3
19#define PARK_SOUTH   4
20#define PARK_FLAT 5
21
22#define MOTOR_DWELL 100
23
24#define MAX_MODBUS_DATA   60
25
26#define HT16K33_DSP_NOBLINK 0   // constants for the half arsed cheapo   display
27#define HT16K33_DSP_BLINK1HZ 4
28#define HT16K33_DSP_BLINK2HZ 2
29#define   HT16K33_DSP_BLINK05HZ 6
30
31
32const byte ENCODER_PINA = 2;  // encoder connects   - interupt pin
33const byte ENCODER_PINB = 3;  // non interupt pin
34const byte   ENCODER_PB = 8;
35
36const byte RELAY_XZ_DIR = 4; // DIR 1    X+ X-  North / South     Was the X+ N relay
37const byte RELAY_XZ_PWM = 5; // PWM 1    Speed North /   South     Was the X- S relay
38const byte RELAY_YZ_PWM = 6; // PWM 2    Speed East   / West       Was the Y+ W relay
39const byte RELAY_YZ_DIR = 7; // DIR 2    Y+ Y-   East / West       Was the Y- E relay
40
41const byte UNUSED09 =  9;  // cycle   timer 26 Hz  38ms period
42const byte UNUSED10 = 10;
43const byte UNUSED11 = 11;
44const   byte WATCHDOG = 12;
45
46
47//L3G gyro;
48LSM303 compass;
49LiquidCrystal_I2C   lcd(0x27);  // Set the LCD I2C address I modified the lib for default wiring
50
51//SFE_BMP180   pressure;
52HT16K33 HT;
53
54Modbus slave1(ID, 1, 0); // this is slave ID and   RS-232 or USB-FTDI
55
56uint8_t time[8];
57int motor_recycle = 0 ;
58char recv[BUFF_MAX];
59unsigned   int recv_size = 0;
60unsigned long prev_millis;
61uint8_t u8state; //!< machine   state
62uint8_t u8query; //!< pointer to message query
63
64struct ts t;             //   MODBUS MAP
65struct ts tc;            // 44 
66int iSave = 0 ;          // 43     
67int iDoSave = 0 ;        // 42
68float T;                 // 40 temperature   of board
69float xzTarget ;         // 38 target for angles
70float yzTarget ;          // 36
71float xzH ;              // 34 hyserisis zone
72float yzH ;              //   32
73float xzAng;             // 30 current angles
74float yzAng;             //   28
75float xzOffset;          // 26 offset xz
76float yzOffset;          // 24   offset yz
77float dyPark;            // 22 parking position
78float dxPark;            //   20 
79float xMinVal ;          // 18 Min and Max values  X - N/S
80float xMaxVal   ;          // 16 
81float yMinVal ;          // 14  Y  -- E/W
82float yMaxVal   ;          // 12 
83float latitude;          // 10
84float longitude;         //   8
85int timezone;            // 7
86int iDayNight ;          // 6
87float solar_az_deg;       // 4
88float solar_el_deg;      // 2
89int iTrackMode ;         // 1 
90int   iMode ;              // 0
91
92int iCycle ;
93int iPMode;
94int iPWM_YZ ;
95int   iPWM_XZ ;
96
97unsigned long tempus;
98int8_t state1 = 0;
99
100void StopYZ(){
101   iPWM_YZ=0 ;
102  motor_recycle = MOTOR_DWELL ;
103}
104void StopXZ(){
105  iPWM_XZ=0   ;
106  motor_recycle = MOTOR_DWELL ;
107}
108
109void ActivateRelays(int iAllStop)   {
110  if (motor_recycle > 0 ){
111    motor_recycle-- ;
112  }
113  if ( iAllStop   == 0 ) {
114    StopYZ() ;
115    StopXZ() ;
116  } else {
117    if (( iPWM_YZ==0   ) && (motor_recycle == 0 )){
118      if (((yzAng  ) < ( yzTarget - yzH )) ) {   //   do Y ie E/W before N/S
119        digitalWrite(RELAY_YZ_DIR, LOW) ;
120        iPWM_YZ=128   ;
121      }
122      if (((yzAng ) > (yzTarget + yzH )) ) {
123        digitalWrite(RELAY_YZ_DIR,   HIGH) ;
124        iPWM_YZ=128 ;
125      }
126    }
127    if ( iPWM_YZ>0 ){
128       if ((yzAng > yzTarget) && ( digitalRead(RELAY_YZ_DIR)==LOW )) {
129        StopYZ()   ;
130      }
131      if ((yzAng < yzTarget) && ( digitalRead(RELAY_YZ_DIR)==HIGH   )) {
132        StopYZ() ;
133      }
134    }
135
136    if (( iPWM_YZ==0)) {  //   if finished on E/W you can do N/S
137        if (( iPWM_XZ==0 ) && (motor_recycle   == 0 )){
138          if ((xzAng < ( xzTarget - xzH ))  )  { // turn on if not in   tolerance
139            digitalWrite(RELAY_XZ_DIR, LOW) ;
140            iPWM_XZ=128   ;
141          }
142          if ((xzAng > ( xzTarget + xzH )) ) { // turn on if   not in tolerance
143            digitalWrite(RELAY_XZ_DIR, HIGH) ;
144            iPWM_XZ=128   ;
145          }
146        }
147    }else{
148      if ((iPWM_XZ>0 )){
149        StopXZ()   ;
150      }
151    }
152    if ( iPWM_XZ>0 ){
153      if ((xzAng > xzTarget )   && ( digitalRead(RELAY_XZ_DIR)==LOW ))  { // if on turn off
154        StopXZ()   ;
155      }
156      if ((xzAng < xzTarget ) && ( digitalRead(RELAY_XZ_DIR)==HIGH   ))  { // if on turn off
157        StopXZ() ;
158      }
159    }
160  }
161  if   (iPWM_XZ>0){
162    iPWM_XZ += 3 ;
163  }
164  if (iPWM_YZ>0){
165    iPWM_YZ +=   3 ;
166  }
167  iPWM_XZ = constrain(iPWM_XZ,0,254);  
168  iPWM_YZ = constrain(iPWM_YZ,0,254);   
169  analogWrite(RELAY_XZ_PWM,iPWM_XZ);
170  analogWrite(RELAY_YZ_PWM,iPWM_YZ);
171}
172
173void   FloatToModbusWords(float src_value , uint16_t * dest_lo , uint16_t * dest_hi ) {
174   uint16_t tempdata[2] ;
175  float *tf ;
176  tf = (float * )&tempdata[0]  ;
177   *tf = src_value ;
178  *dest_lo = tempdata[1] ;
179  *dest_hi = tempdata[0] ;
180}
181float   FloatFromModbusWords( uint16_t dest_lo , uint16_t dest_hi ) {
182  uint16_t tempdata[2]   ;
183  float *tf ;
184  tf = (float * )&tempdata[0]  ;
185  tempdata[1] = dest_lo   ;
186  tempdata[0] = dest_hi  ;
187  return (*tf) ;
188}
189
190// Arduino doesnt   have these to we define from a sandard libruary
191float arcsin(float x) {
192  return   (atan(x / sqrt(-x * x + 1)));
193}
194float arccos(float x) {
195  return (atan(x   / sqrt(-x * x + 1)) + (2 * atan(1)));
196}
197// fractional orbital rotation in   radians
198float gama(struct ts *tm) {
199  return ((2 * PI / 365 ) *  DayOfYear(tm->year   , tm->mon , tm->mday , tm->hour , tm->min ));
200}
201// equation of rime
202float   eqTime(float g) {
203  return (229.18 * ( 0.000075 + ( 0.001868 * cos(g)) - (0.032077   * sin(g)) - (0.014615 * cos (2 * g)) - (0.040849 * sin(2 * g))));
204}
205// declination   of sun in radians
206float Decl(float g) {
207  return ( 0.006918 - (0.399912 *   cos(g)) + (0.070257 * sin(g)) - (0.006758 * cos(2 * g)) + ( 0.000907 * sin(2 * g))   - ( 0.002697 * cos(3 * g)) + (0.00148 * sin(3 * g)) );
208}
209float TimeOffset(float   longitude , struct ts *tm ,  int timezone ) {
210  float dTmp ;
211  dTmp = (-4.0   * longitude ) + (60 * timezone) - eqTime(gama(tm)) ;
212  return (dTmp);
213}
214
215float   TrueSolarTime(float longitude , struct ts *tm ,  int timezone ) {
216  float dTmp   ;
217  dTmp = ( 60.0 * tm->hour ) + (1.0 * tm->min) + (1.0 * tm->sec / 60) - TimeOffset(longitude,   tm, timezone) ;
218  return (dTmp);
219}
220float HourAngle(float longitude , struct   ts *tm ,  int timezone) {
221  float dTmp;
222  dTmp = (TrueSolarTime(longitude,   tm, timezone) / 4 ) - 180 ; // 720 minutes is solar noon -- div 4 is 180
223  return   (dTmp);
224}
225// Hour angle for sunrise and sunset only
226float HA (float lat   , struct ts *tm ) {
227  float latRad ;
228  latRad = lat * 2 * PI / 360 ;
229  return   ( acos((cos(90.833 * PI / 180 ) / ( cos(latRad) * cos(Decl(gama(tm)))) - (tan(latRad)   * tan(Decl(gama(tm)))))) / PI * 180  );
230}
231
232float Sunrise(float longitude   , float lat , struct ts *tm , int timezone) {
233  return (720 - ( 4.0 * (longitude   + HA(lat, tm))) + (60 * timezone) - eqTime(gama(tm))  ) ;
234}
235float Sunset(float   longitude , float lat , struct ts *tm , int timezone) {
236  return (720 - ( 4.0   * (longitude - HA(lat, tm))) + (60 * timezone) - eqTime(gama(tm))  ) ;
237}
238float   SNoon(float longitude , float lat , struct ts *tm , int timezone) {
239  return   (720 - ( 4.0 * (longitude  + (60 * timezone) - eqTime(gama(tm))))  ) ;
240}
241
242float   SolarZenithRad(float longitude , float lat , struct ts *tm , int timezone) {
243   float latRad ;
244  float decRad ;
245  float HourAngleRad ;
246  float dTmp ;
247
248   latRad = lat * 2 * PI / 360 ;
249  decRad = Decl(gama(tm));
250  HourAngleRad   = HourAngle (longitude , tm , timezone ) * PI / 180 ;
251  dTmp = acos((sin(latRad)   * sin(decRad)) + (cos(latRad) * cos(decRad) * cos(HourAngleRad)));
252  return (dTmp)   ;
253
254}
255float SolarElevationRad(float longitude , float lat , struct ts *tm   ,  int timezone ) {
256  return ((PI / 2) - SolarZenithRad(longitude , lat , tm   , timezone )) ;
257}
258
259float SolarAzimouthRad(float longitude , float lat ,   struct ts *tm ,  int timezone) {
260  float latRad ;
261  float decRad ;
262  float   solarzenRad ;
263  float HourAngleRad ;
264  float dTmp ;
265  latRad = lat * 2   * PI / 360 ;
266  decRad = Decl(gama(tm));
267  solarzenRad = SolarZenithRad ( longitude   , lat , tm , timezone ) ;
268  HourAngleRad = HourAngle (longitude , tm , timezone   ) * PI / 180 ;
269  dTmp = acos(((sin(decRad) * cos(latRad)) - (cos(HourAngleRad)   * cos(decRad) * sin(latRad))) / sin(solarzenRad)) ;
270  if ( HourAngleRad < 0 )   {
271    return (dTmp) ;
272  } else {
273    return ((2 * PI) - dTmp) ;
274  }
275}
276
277float   LoadFloatFromEEPROM(int address,float minval,float maxval, float defaultval){
278float   tmp ;  
279  EEPROM.get(0 + (address * sizeof(float)) , tmp );
280  if (( tmp <   minval ) || ( tmp > maxval )|| (NumberOK(tmp) == 1)) {
281    tmp = defaultval ;
282     EEPROM.put(0 + (address * sizeof(float)) , tmp );
283  }
284  return(tmp);  
285}
286int   LoadIntFromEEPROM(int address,int minval,int maxval, int defaultval){
287int tmp   ;  
288  EEPROM.get(0 + (address * sizeof(float)) , tmp );
289  if (( tmp < minval   ) || ( tmp > maxval )) {
290    tmp = defaultval ;
291    EEPROM.put(0 + (address   * sizeof(float)) , tmp );
292  }
293  return(tmp);  
294}
295
296int NumberOK (float   target) {
297  int tmp = 0 ;
298  tmp = isnan(target);
299  if ( tmp != 1 ) {
300     tmp = isinf(target);
301  }
302  return (tmp);
303}
304
305void IncFloat(float   * target, boolean bInc, float increment, float minval, float maxval , boolean bWrap){
306     if( bInc  ){
307      *target += increment ;  
308    }else{
309      *target   -= increment ;  
310    }
311    if ( *target > maxval ){
312      if ( bWrap  ){
313         *target = minval ;
314      }else{
315        *target = maxval ;
316      }
317     }
318    if ( *target < minval ){
319      if ( bWrap  ){
320        *target   = maxval ;
321      }else{
322        *target = minval ;
323      }
324    }
325}
326void   IncInt(int * target, boolean bInc, int increment, int minval, int maxval , boolean   bWrap){
327    if( bInc  ){
328      *target += increment ;  
329    }else{
330       *target -= increment ;  
331    }
332    if ( *target > maxval ){
333      if   ( bWrap  ){
334        *target = minval ;
335      }else{
336        *target = maxval   ;
337      }
338    }
339    if ( *target < minval ){
340      if ( bWrap  ){
341         *target = maxval ;
342      }else{
343        *target = minval ;
344      }
345     }  
346}
347void IncUint8_t (uint8_t * target , boolean bInc, int increment,   int minval, int maxval , boolean bWrap){
348    if( bInc  ){
349      *target +=   increment ;  
350    }else{
351      *target -= increment ;  
352    }
353    if   ( *target > maxval ){
354      if ( bWrap  ){
355        *target = minval ;
356       }else{
357        *target = maxval ;
358      }
359    }
360    if ( *target   < minval ){
361      if ( bWrap  ){
362        *target = maxval ;
363      }else{
364         *target = minval ;
365      }
366    }  
367}
368void StdFlloatValueDisplay(float   target, char message[]){
369  lcd.setCursor ( 0, 1 );       // line 1
370  lcd.print(message)   ;
371  lcd.print(target) ;
372  lcd.setCursor ( 0, 2 );       // line 2
373  lcd.print(   "PB to set ") ;
374}
375 
376
377void setup() {
378  int led ;
379  
380  Wire.begin();
381   lcd.begin(20, 4);
382  lcd.home();
383  lcd.setBacklightPin(3, NEGATIVE);
384   lcd.noCursor(); 
385
386  MCUSR &= ~_BV(WDRF);
387  wdt_disable();
388  
389  compass.init();
390   compass.enableDefault();
391  compass.setTimeout(1000);
392  
393  pinMode(RELAY_XZ_DIR,   OUTPUT); // Outputs for PWM motor control
394  pinMode(RELAY_XZ_PWM, OUTPUT); //   
395  pinMode(RELAY_YZ_PWM, OUTPUT); //
396  pinMode(RELAY_YZ_DIR, OUTPUT); // 
397   iPWM_YZ = 0 ;
398  iPWM_XZ = 0 ;
399  pinMode(13, OUTPUT); //
400  digitalWrite(13,   HIGH );
401  ActivateRelays(0); // call an all stop first
402
403  Serial.begin(9600)   ;  // use as a diagnostic port
404  slave1.begin( 9600 ); // RS-232 to base of tower
405   tempus = millis() + 100;
406
407  pinMode(ENCODER_PINA, INPUT_PULLUP);
408  pinMode(ENCODER_PINB,   INPUT_PULLUP);
409  pinMode(ENCODER_PB, INPUT_PULLUP);
410  attachInterrupt(0, counter,   FALLING);
411
412  pinMode(UNUSED09, OUTPUT);  // unused so dont leave floating   set as output
413  pinMode(UNUSED10, OUTPUT);  // 
414  pinMode(UNUSED11, OUTPUT);   // 
415  pinMode(WATCHDOG, OUTPUT);  // 
416  digitalWrite(WATCHDOG,HIGH);
417
418   compass.m_min = (LSM303::vector<int16_t>) {-3848, -1822, -1551 };   // calibration   figures are empirical
419  compass.m_max = (LSM303::vector<int16_t>) { +3353, +5127,   +5300};
420
421  xzH = LoadFloatFromEEPROM(0,0.1,20.0,4.0);  // hysterisis NS
422   yzH = LoadFloatFromEEPROM(1,0.1,20.0,4.0);  //    ""      EW
423
424  dyPark   = LoadFloatFromEEPROM(2,-70.0,50.0,0);  
425  dxPark = LoadFloatFromEEPROM(3,-5.0,50.0,0.0);   
426  
427  xzOffset = LoadFloatFromEEPROM(4,-90.0,90.0,0);  // NS
428  yzOffset   = LoadFloatFromEEPROM(5,-90.0,90.0,0);  // EW
429
430  xzTarget = LoadFloatFromEEPROM(6,-90.0,90.0,0);   // NS
431  yzTarget = LoadFloatFromEEPROM(7,-90.0,90.0,0);  // EW
432
433  xMinVal   = LoadFloatFromEEPROM(8,-10.0,60.0,0.0);   // NS
434  xMaxVal = LoadFloatFromEEPROM(9,-10.0,60.0,45);
435
436   yMinVal = LoadFloatFromEEPROM(10,-70.0,50.0,-65);  // EW
437  yMaxVal = LoadFloatFromEEPROM(11,-70.0,50.0,45);
438
439   iMode = 0 ;
440  iTrackMode = LoadIntFromEEPROM(12,-1,4,0);
441  
442//  latitude   = -34.051219 ;
443//  longitude = 142.013618 ;
444//  timezone = 10 ;
445   
446   latitude = LoadFloatFromEEPROM(13,-90.0,90.0,-34.051219);
447  longitude = LoadFloatFromEEPROM(14,-180.0,180.0,142.013618);
448   timezone = LoadIntFromEEPROM(15,0,23,10);
449   
450  DS3231_init(DS3231_INTCN);
451   DS3231_get(&tc);
452  DS3231_get(&t);
453  lcd.clear();
454  HT.begin(0x00);
455   for (led = 0; led < 127; led++) {
456    HT.clearLedNow(led);
457  }
458  wdt_enable(WDTO_4S);
459}
460
461void   loop() {
462  float P;
463  float sunInc;
464  float sunAng;
465  float xzRatio;
466   float yzRatio;
467  char buff[BUFF_MAX];
468  float decl ;
469  float eqtime ;
470   float dTmp ;
471  float ha ;
472  float heading ;
473  float sunrise ;
474  float   sunset ;
475  float tst ;
476  float sunX ;
477
478  compass.read();  // this reads   all 6 channels
479//  compass.readAcc();
480//  compass.readMag();
481  digitalWrite(UNUSED09,!digitalRead(UNUSED09));    // toggle this output so I can measure the cycle time with a scope
482  
483   T = DS3231_get_treg();
484  heading = compass.heading((LSM303::vector<int>) {   1, 0, 0 });
485  
486  if (( compass.a.z != 0) && (!compass.timeoutOccurred() ))   {
487    xzRatio = (float)compass.a.y / abs((float)compass.a.z) ;   //  yep if   your sharp you can see I swapped axis orientation late in the programming
488    yzRatio   = (float)compass.a.x / abs((float)compass.a.z) ;
489    xzAng = ((float)atan(xzRatio)   / PI * -180 ) + xzOffset ;   // good old offsets or fudge factors
490    yzAng =   ((float)atan(yzRatio) / PI * 180 ) + yzOffset ;
491    digitalWrite(13, LOW);
492   }else{                                                       // try restarting   the compass/accelerometer modual - cos he gone walkabout...
493    Wire.begin();    // reset the I2C
494    compass.init();
495    compass.enableDefault();
496    compass.setTimeout(1000);                                   // BTW I fixed up the int / long issue in the   time out function in the LM303 lib I was using
497    compass.m_min = (LSM303::vector<int16_t>)   {-3848, -1822, -1551 };   // calibration figures are empirical (just whirl it around   a bit and records the min max !!)
498    compass.m_max = (LSM303::vector<int16_t>)   { +3353, +5127, +5300 };
499    if (tc.sec % 2 == 0 ) {
500      digitalWrite(13,   HIGH);
501    } else {
502      digitalWrite(13, LOW);
503    }
504    HT.begin(0x00);
505   }
506
507  DS3231_get(&tc);
508
509  solar_az_deg = SolarAzimouthRad(longitude,   latitude, &tc, timezone) * 180 / PI ;
510  solar_el_deg = SolarElevationRad(longitude,   latitude, &tc, timezone) * 180 / PI ;
511
512  decl = Decl(gama(&tc)) * 180 / PI   ;
513  ha = HourAngle (longitude , &tc , timezone )  ;
514  sunrise = Sunrise(longitude,   latitude, &tc, timezone) ;
515  sunset = Sunset(longitude, latitude, &tc, timezone);
516   tst = TrueSolarTime(longitude, &tc, timezone);
517  sunX = abs(latitude) + decl   ;
518  if (solar_el_deg >= 0 ){           // day
519    iDayNight = 1 ;
520  }else{                              // night
521    iDayNight = 0 ;
522  }
523
524  switch   (iTrackMode) {
525    case 4: // both axis to park
526      yzTarget = dyPark ;   // night park position  E/W
527      xzTarget = dxPark ;  // night park position   N/S
528      break ;
529    case 3: // both axis off no tracking
530      break   ;
531    case 2: // xz tracking  NS
532      if ( iDayNight == 1 ) {
533        xzTarget   = sunX ; // need to map the coordinate system correctly
534      } else {
535        xzTarget   = dxPark ;  // night park position
536      }
537      break;
538    case 1:  //   yz tracking   EW
539      if (iDayNight == 1) {
540        yzTarget = ha ;
541      }   else {
542        yzTarget = dyPark ;  // night park position
543      }
544      break;
545     case -1: // set target to tracking and park both at nigh
546      if (iDayNight   == 1) {
547        yzTarget = ha ;
548        xzTarget = sunX ; // need to map the   coordinate system correctly
549      } else {
550        yzTarget = dyPark ;  //   night park position  E/W
551        xzTarget = dxPark ;  // night park position   N/S
552      }
553      break;
554    default: // set target to tracking
555      if   (iDayNight == 1) {
556        yzTarget = ha ;
557        xzTarget = sunX ; // need   to map the coordinate system correctly
558      } else {
559        yzTarget = dyPark   ;  // night park position (dont park the other - leave till morning)
560      }
561       break;
562  }
563  xzTarget = constrain(xzTarget,xMinVal,xMaxVal);   // constain   function... very cool - dont leave home without it !
564  yzTarget = constrain(yzTarget,yMinVal,yMaxVal);
565   
566  if ( iPMode != iMode ) { // clear screen after first change of mode
567     lcd.clear();
568    if ((( iPMode < 1 ) ||  ( iPMode > 8 )) && ( iMode > 0 )   && (iMode < 9 )) {
569      DS3231_get(&t);  // when going into time edit mode update   the time - then leave it allone to be worked on by the editor
570    }
571    iPMode   = iMode ;
572  }
573  if ( iCycle != tc.sec )  {     //only update once a second
574     digitalWrite(WATCHDOG,LOW);  // external watch dog pin
575    wdt_reset();                 //   reset internal watchdog - good puppy
576    //   lcd.clear();
577    if ( iMode   > 0 ){
578      lcd.setCursor ( 0, 3 );       // line 3
579      lcd.print( "Mode   ") ;
580      lcd.print(iMode) ;
581      lcd.print( " " ) ;        
582      switch   (tc.sec % 2 ){
583        case 1:
584          lcd.print( "|" ) ;        
585         break;
586        default:
587          lcd.print( "-" ) ;        
588         break;
589      }
590    }
591    lcd.setCursor ( 0, 0 );      // line 0
592     switch (iMode) {
593      case 22:
594        StdFlloatValueDisplay(yMaxVal,"Y   Max ");
595      break;
596      case 21:
597        StdFlloatValueDisplay(yMinVal,"Y   Min ");
598      break;
599      case 20:
600        StdFlloatValueDisplay(xMaxVal,"X   Max ");
601      break;
602      case 19:
603        StdFlloatValueDisplay(xMinVal,"X   Min ");
604      break;
605      case 23:
606        lcd.setCursor ( 0, 0 );
607         lcd.print("Lat  ");
608        lcd.print(latitude,6);
609        lcd.setCursor   ( 0, 1 );
610        lcd.print("Long ");
611        lcd.print(longitude,6);
612         lcd.setCursor ( 0, 2 );
613        lcd.print("Time Zone ");
614        lcd.print(timezone);
615         break;
616      case 24:
617        //        lcd.clear();
618        lcd.setCursor   ( 0, 0 );
619        lcd.print("X/Z ");
620        if ( xzAng > 0 ) {
621          lcd.print("+");
622         }
623        lcd.print(xzAng);
624        lcd.setCursor ( 10, 0 );
625        lcd.print("Y/Z   ");
626        if ( yzAng > 0 ) {
627          lcd.print("+");
628        }
629         lcd.print(yzAng);
630        lcd.setCursor ( 0, 1 );
631        lcd.print("TX   ");
632        if (( xzTarget) > 0 ) {
633          lcd.print("+");
634        }
635         lcd.print(( xzTarget));
636        lcd.setCursor ( 10, 1 );
637        lcd.print("TY   ");
638        if ((  yzTarget) > 0 ) {
639          lcd.print("+");
640        }
641         lcd.print((  yzTarget));
642        lcd.setCursor ( 0, 2 );
643        lcd.print("DX   ");
644        dTmp = ( xzAng - xzTarget) ;
645        if (dTmp > 0 ) {
646          lcd.print("+");
647         }
648        lcd.print(dTmp);
649        lcd.setCursor ( 10, 2 );
650        lcd.print("DY   ");
651        dTmp = ( yzAng - yzTarget) ;
652        if (dTmp > 0 ) {
653          lcd.print("+");
654         }
655        lcd.print(dTmp);
656
657        lcd.setCursor ( 10, 3 );       //   line 2
658        lcd.print( "T ") ;
659        lcd.print(T) ;
660
661        lcd.setCursor   ( 18, 3 );       // line 2
662        if ( iPWM_YZ == 0 ) {
663          lcd.print(   " ") ;
664        }else{
665          if (( digitalRead(RELAY_YZ_DIR) == LOW ))   {
666            lcd.print( "W") ;
667          }else{
668            lcd.print(   "E") ;            
669          }
670        }
671        lcd.setCursor ( 19,   3 );       // line 2
672        if ( iPWM_XZ == 0 ) {
673          lcd.print( "   ") ;
674        }else{
675          if (( digitalRead(RELAY_XZ_DIR) == LOW )) {
676             lcd.print( "N") ;
677          }else{
678            lcd.print( "S")   ;            
679          }
680        }
681        
682        break;
683      case   18: // SAVE ALL
684        lcd.setCursor ( 0, 1 );       // line 1
685        if   ( iSave != 0 ) {
686          lcd.print( "SAVE REQUIRED") ;
687        } else   {
688          lcd.print( "### SAVED ###") ;
689        }
690        if ( iDoSave   == 2 ) {
691          EEPROM.put( 0 , xzH );
692          EEPROM.put(0 + (1 * sizeof(float))   , yzH );
693          EEPROM.put(0 + (2 * sizeof(float)) , dyPark );
694          EEPROM.put(0   + (3 * sizeof(float)) , dxPark );
695          EEPROM.put(0 + (4 * sizeof(float))   , xzOffset );
696          EEPROM.put(0 + (5 * sizeof(float)) , yzOffset );
697           EEPROM.put(0 + (6 * sizeof(float)) , xzTarget );
698          EEPROM.put(0   + (7 * sizeof(float)) , yzTarget );
699          EEPROM.put(0 + (8 * sizeof(float))   , xMinVal );
700          EEPROM.put(0 + (9 * sizeof(float)) , xMaxVal );
701          EEPROM.put(0   + (10 * sizeof(float)) , yMinVal );
702          EEPROM.put(0 + (11 * sizeof(float))   , yMaxVal );
703          EEPROM.put(0 + (12 * sizeof(float)) , iTrackMode );
704           EEPROM.put(0 + (13 * sizeof(float)) , latitude );
705          EEPROM.put(0   + (14 * sizeof(float)) , longitude );
706          EEPROM.put(0 + (15 * sizeof(float))   , timezone );
707          iDoSave = 0 ;
708          iSave = 0 ;
709        }
710         lcd.setCursor ( 0, 2 );       // line 2
711        lcd.print( "PB to Save   ALL") ;
712      break ;
713      case 17:     // yzH hysterisis
714        StdFlloatValueDisplay(yzH,"Y   Hysteresis ");
715      break;
716      case 16:     // xzH  hysterisis
717        StdFlloatValueDisplay(xzH,"X   Hysteresis ");
718      break;
719      case 15:     // yOffset
720        StdFlloatValueDisplay(yzOffset,"Y   Offset ");
721      break;
722      case 14:     // xOffset
723        StdFlloatValueDisplay(xzOffset,"X   Offset ");
724      break;
725      case 13:
726        lcd.setCursor ( 0, 1 );        // line 1
727        lcd.print(iTrackMode) ;
728        lcd.print( " - ")   ;
729        PrintTrackerMode(iTrackMode);
730        lcd.setCursor ( 0, 2 );       //   line 2
731        lcd.print( "PB change track mode") ;
732        break;
733      case   12:
734        StdFlloatValueDisplay(dxPark,"X Park ");
735
736        break;
737       case 11:
738        StdFlloatValueDisplay(dyPark,"Y Park ");
739
740        break;
741       case 10:
742        StdFlloatValueDisplay(yzTarget,"Y ");
743        lcd.print(   " WE angle") ;
744        break;
745      case 9:
746        StdFlloatValueDisplay(xzTarget,"X   ");
747        lcd.print( " NS angle") ;
748        break;
749      case 8:
750         snprintf(buff, BUFF_MAX, "%d/%02d/%02d %02d:%02d:%02d", tc.year, tc.mon,   tc.mday , tc.hour, tc.min, tc.sec);
751        lcd.print(buff) ;
752        lcd.setCursor   ( 0, 1 );       // line 2
753        snprintf(buff, BUFF_MAX, "%d/%02d/%02d %02d:%02d:%02d",   t.year, t.mon, t.mday , t.hour, t.min, t.sec);
754        lcd.print(buff) ;
755         lcd.setCursor ( 0, 2 );       // line 2
756        lcd.print( "Press PB   to set time ") ;
757        if ( not  digitalRead(ENCODER_PB) ) {
758          DS3231_set(t);
759         }
760        break;
761      case 1:
762      case 2:
763      case 3:
764       case 4:
765      case 5:
766      case 6:
767      case 7:
768        snprintf(buff,   BUFF_MAX, "%d/%02d/%02d %02d:%02d:%02d", t.year, t.mon, t.mday , t.hour, t.min,   t.sec);
769        lcd.print(buff) ;
770        if (iMode == 7) {
771          lcd.setCursor   ( 0, 1 );
772          lcd.print( "Day - > ") ;
773          PrintDay(t.wday);
774         }
775        lcd.setCursor ( 0, 2 );
776        lcd.print( "Hold in set")   ;
777        lcd.setCursor ( 10, 3 ) ;
778        switch (iMode) {
779          case   1:
780            lcd.print( "Year") ;
781            break;
782          case   2:
783            lcd.print( "Month") ;
784            break;
785          case   3:
786            lcd.print( "Day") ;
787            break;
788          case   4:
789            lcd.print( "Hour") ;
790            break;
791          case   5:
792            lcd.print( "Min") ;
793            break;
794          case   6:
795            lcd.print( "Sec") ;
796            break;
797          case   7:
798            lcd.print( "WDay") ;
799            break;
800        }
801         //    lcd.noBlink();
802        break;
803      default:   // 0
804        DS3231_get(&tc);                                                      // update the time registers
805         snprintf(buff, BUFF_MAX, "%d/%02d/%02d %02d:%02d:%02d", tc.year, tc.mon,   tc.mday , tc.hour, tc.min, tc.sec);
806        lcd.print(buff) ;
807        lcd.setCursor   ( 0, 1 );       // line 2 has the Az and El
808        lcd.print( "Az ") ;
809         lcd.print(solar_az_deg) ;
810        lcd.setCursor ( 10, 1 );
811        lcd.print(   "El ") ;
812        lcd.print(solar_el_deg) ;
813
814        lcd.setCursor ( 0,   2 );
815        lcd.print("D ");
816        if ( decl > 0 ) {
817          lcd.print("+");
818         }
819        lcd.print(decl);
820        lcd.setCursor ( 10, 2 );
821        lcd.print("H   ");
822        if ( ha > 0 ) {
823          lcd.print("+");
824        }
825         lcd.print(ha);
826        lcd.setCursor ( 0, 3 );   // third line is sunset   and sunrise
827        snprintf(buff, BUFF_MAX, "%02d:%02d", HrsSolarTime(sunrise),   MinSolarTime(sunrise));
828        lcd.print(buff) ;
829        if (iDayNight ==   1) {
830          lcd.setCursor ( 8, 3 );
831          lcd.print(" DAY ") ;
832         } else {
833          lcd.setCursor ( 7, 3 );
834          lcd.print("NIGHT")   ;
835        }
836        lcd.setCursor ( 15, 3 );
837        snprintf(buff, BUFF_MAX,   "%02d:%02d", HrsSolarTime(sunset), MinSolarTime(sunset));
838        lcd.print(buff)   ;
839        break;
840    }
841
842    iCycle = tc.sec ;
843    DisplayMeatBall()   ;
844  }
845
846  if (((tc.hour > 19 ) || ( tc.hour < 5 )) && (iTrackMode < 3))   {
847    ActivateRelays(0) ;  // power down at night if in tracking mode
848  }else{
849     ActivateRelays(1) ;    
850  }
851  digitalWrite(WATCHDOG,HIGH); // nice doggy
852   
853  state1 = slave1.poll( (uint16_t*)&iMode, MAX_MODBUS_DATA );
854
855  switch   (state1) {
856    case EXC_ADDR_RANGE:
857      //      Serial.println("EXC_ADDR_RANGE   PORT 1");
858      break;
859    case EXC_FUNC_CODE:
860      //    Serial.println("EXC_FUNC_CODE   PORT 1");
861      break;
862    case EXC_REGS_QUANT:
863      //      Serial.println("EXC_REGS_QUANT   PORT 1");
864      break;
865  }
866
867}
868
869
870
871
872
873
874float DayOfYear(uint16_t   iYear , uint8_t iMon , uint8_t iDay , uint8_t iHour , uint8_t iMin ) {
875  int   i ;
876  float iTDay ;
877
878  iTDay = iDay - 1 ;  // this is zero referenced
879   for ( i = 1 ; i < iMon ; i++ ) {
880    switch (i) {
881      case 1:
882      case   3:
883      case 5:
884      case 7:
885      case 8:
886      case 10:
887      case   12:
888        iTDay += 31 ;
889        break;
890      case 4:
891      case 6:
892       case 9:
893      case 11:
894        iTDay += 30 ;
895        break;
896      case   2 :
897        if ((iYear % 4) == 0 ) {
898          iTDay += 29 ;
899        }   else {
900          iTDay += 28 ;
901        }
902        break;
903    }
904  }
905   iTDay += (( 1.0 * iHour - 12 ) / 24 ) ;
906  //  iDay +=  1.0 * iMin  / 1440 ;
907   return (iTDay);
908}
909
910void PrintTrackerMode(int iTarget) {
911  switch (iTarget)   {
912    case 1:
913      lcd.print( "EW Only") ;
914      break;
915    case   2:
916      lcd.print( "NS Only") ;
917      break;
918    case 3:
919      lcd.print(   "None") ;
920      break;
921    case 4:
922      lcd.print( "Both Park") ;
923       break;
924    case -1:   
925      lcd.print( "2P ") ;
926    default:
927       lcd.print( "Both Track") ;
928      break;
929  }
930}
931
932
933void PrintDay(int   iTarget) {
934  switch (iTarget) {
935    case 1:
936      lcd.print( "Sun") ;
937       break;
938    case 2:
939      lcd.print( "Mon") ;
940      break;
941    case   3:
942      lcd.print( "Tue") ;
943      break;
944    case 4:
945      lcd.print(   "Wed") ;
946      break;
947    case 5:
948      lcd.print( "Thr") ;
949      break;
950     case 6:
951      lcd.print( "Fri") ;
952      break;
953    case 7:
954      lcd.print(   "Sat") ;
955      break;
956  }
957}
958
959int HrsSolarTime(float target) {
960   int i ;
961  i = target ;
962  return (  i / 60 );
963}
964int MinSolarTime(float   target) {
965  int i ;
966  i = target  ;
967  return (   i % 60 );
968}
969
970void   counter()
971{
972
973  if ( ( prev_millis + 100 ) < millis() ) {
974    if ( digitalRead(ENCODER_PB)   ) {
975      IncInt(&iMode,digitalRead(ENCODER_PINB),1,0,24,true);
976    } else   {
977      if ((iMode > 8 ) && (iMode < 23) && (iMode != 18)) {
978        iSave   = 1;
979      }
980      switch (iMode) {  // setting the time and date etc
981         case 1:   // year
982          IncInt(&t.year ,digitalRead(ENCODER_PINB),1,2016,3116,false);
983           break;
984        case 2:   // month
985          IncUint8_t(&t.mon ,digitalRead(ENCODER_PINB),1,1,12,true);
986           break;
987        case 3:  // day
988          IncUint8_t(&t.mday ,digitalRead(ENCODER_PINB),1,1,31,true);
989           break;
990        case 4:  // hour
991          IncUint8_t(&t.hour ,digitalRead(ENCODER_PINB),1,0,23,true);
992           break;
993        case 5:
994          IncUint8_t(&t.min ,digitalRead(ENCODER_PINB),1,0,59,true);
995           break;
996        case 6:
997          IncUint8_t(&t.sec ,digitalRead(ENCODER_PINB),1,0,59,true);
998           break;
999        case 7:
1000          IncUint8_t(&t.wday ,digitalRead(ENCODER_PINB),1,1,7,true);
1001           break;
1002        case 9:
1003          IncFloat(&xzTarget,digitalRead(ENCODER_PINB),1,0,90,false);
1004           break;
1005        case 10:
1006          IncFloat(&yzTarget,digitalRead(ENCODER_PINB),1,-80,80,false);
1007           break;
1008        case 11:
1009          IncFloat(&dyPark,digitalRead(ENCODER_PINB),1,-80,80,false);
1010           break;
1011        case 12:
1012          IncFloat(&dxPark,digitalRead(ENCODER_PINB),1,-80,80,false);
1013           break;
1014        case 13:
1015          IncInt(&iTrackMode,digitalRead(ENCODER_PINB),1,-1,4,true);
1016           break;
1017        case 14:
1018          IncFloat(&xzOffset,digitalRead(ENCODER_PINB),0.5,-20,20,false);
1019           break;
1020        case 15:
1021          IncFloat(&yzOffset,digitalRead(ENCODER_PINB),0.5,-20,20,false);
1022           break;
1023        case 16:
1024          IncFloat(&xzH,digitalRead(ENCODER_PINB),0.25,-20,20,false);
1025           break;
1026        case 17:
1027          IncFloat(&yzH,digitalRead(ENCODER_PINB),0.25,-20,20,false);
1028           break;
1029        case 18:
1030          if (( iDoSave == 0 ) && ( iSave   != 0 )) {
1031            iDoSave = 2 ;
1032          }
1033        break;
1034        case   19:
1035          IncFloat(&xMinVal,digitalRead(ENCODER_PINB),1.0,-10,50,false);
1036         break;
1037        case 20:
1038          IncFloat(&xMaxVal,digitalRead(ENCODER_PINB),1.0,-10,50,false);
1039         break;
1040        case 21:
1041          IncFloat(&yMinVal,digitalRead(ENCODER_PINB),1.0,-70,50,false);
1042         break;
1043        case 22:
1044          IncFloat(&yMaxVal,digitalRead(ENCODER_PINB),1.0,-70,50,false);
1045         break;
1046      }
1047    }
1048    prev_millis = millis();
1049  } else {
1050     if ( prev_millis > ( millis() + 1000 ) ) {
1051      prev_millis = millis();
1052     }
1053  }
1054
1055}
1056
1057float sign(float target) {
1058  if (target > 0 ) {
1059     return (1);
1060  } else {
1061    if (target < 0 ) {
1062      return (-1);
1063     } else {
1064      return (0);
1065    }
1066  }
1067}
1068
1069void DisplayMeatBall()   {
1070  int  pos , led , x , y;
1071  float dx , dy ;
1072  float dxa , dya ;
1073
1074   HT.setBrightness(15);
1075
1076  dx = xzAng - xzTarget ;
1077  dy = yzAng - yzTarget   ;
1078  dxa = abs(dx) ;
1079  dya = abs(dy) ;
1080  if (dxa < 6) {
1081    x = 0 ;
1082   } else {
1083    if (dxa < 12) {
1084      x = sign(dx);
1085    } else {
1086      if   (dxa < 50) {
1087        x = 2 * sign(dx);
1088      } else {
1089        x = 3 * sign(dx);
1090       }
1091    }
1092  }
1093  if (dya < 6) {
1094    y = 0 ;
1095  } else {
1096    if   (dya < 12) {
1097      y = sign(dy);
1098    } else {
1099      if (dya < 25) {
1100         y = 2 * sign(dy);
1101      } else {
1102        y = 3 * sign(dy);
1103      }
1104     }
1105  }
1106  pos = 27 ; // netral position
1107  pos += (y * 8) ; // add or   sumtract the x in range of -3 to +3
1108  pos += (x ) ; // add or sumtract 8 * y   or y in range of -3 to +3
1109  for (led = 0; led < 63; led++) {
1110    switch (led){
1111        case 0:
1112       case 7:
1113       case 56:
1114       case 63:
1115       break;
1116        default:
1117         HT.clearLedNow(MapLedNo(led));
1118       break;  
1119     }
1120  }
1121
1122  HT.setLedNow(MapLedNo(0)); // turn on four courners
1123  HT.setLedNow(MapLedNo(7));
1124   HT.setLedNow(MapLedNo(56));
1125  HT.setLedNow(MapLedNo(63));
1126
1127
1128  //  HT.setLedNow(MapLedNo(tc.sec));
1129   if ((iPWM_YZ == 0) && (iPWM_XZ == 0)) {
1130    HT.setBlinkRate(HT16K33_DSP_NOBLINK);   // not attempting to move
1131    if (tc.sec % 2 == 0 ) {
1132      HT.setLedNow(MapLedNo(pos   + 0)); // display the meatball
1133      HT.setLedNow(MapLedNo(pos + 9));
1134    }else{
1135       HT.setLedNow(MapLedNo(pos + 1));
1136      HT.setLedNow(MapLedNo(pos + 8));
1137     }
1138  } else {
1139    HT.setBlinkRate(HT16K33_DSP_BLINK2HZ); //moving so blink   meat ball
1140    HT.setLedNow(MapLedNo(pos + 0)); // display the meatball
1141    HT.setLedNow(MapLedNo(pos   + 1));
1142    HT.setLedNow(MapLedNo(pos + 8));
1143    HT.setLedNow(MapLedNo(pos   + 9));
1144  }
1145}
1146
1147
1148int MapLedNo(int target)  // this compensates for   the screwy setup of the matrix driver to the chip
1149{
1150  int row ;
1151  int col   ;
1152  row = target / 8 ;
1153  col = target % 8 ;
1154  if (col == 0 ) {
1155    return   ((row * 16 ) + 7) ;
1156  } else {
1157    return ((row * 16 ) + col - 1) ;
1158  }
1159}
1160
1161