 //
-// LED7Seg Demo Example
+// Robotics PWM Motor Controller
 //
-// Copyright (C) 2016 Lennie Araki. All rights reserved
+// Copyright (C) 2016-2020 Lennie Araki. All rights reserved
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

/* returns change in encoder state (-1,0,1) */
int8_t read_encoder()
{
	// remember previou state and ORin new state
	old_AB = (old_AB << 2) | (PINC & 0x03); //remember previous state
	return enc_states[old_AB & 0x0f];
}

void loop()
{
	static unsigned long ten_msec = millis();
	static int counter = 0;
	static int encoder = 0;
	
//	int pot = analogRead(0);
	encoder += read_encoder();

	if (millis() >= ten_msec) {
		ten_msec += 100;
		++counter;
		if (counter < 200)
		{
			int segment = counter & 7;
			int digit = (counter / 8) % LED_DIGITS;
			for (byte d = 0; d < LED_DIGITS; ++d)
			{
				byte raw[LED_DIGITS];
				raw[d] = (d == digit) ?
					((SEG_B << segment) - 1) : SEG_NONE;
				led7seg.showRaw(raw);
			}
		}
#ifdef LEDFONT
		else if (counter < 400)
		{
			static const char banner[] = "The Quick Brown Fox Jumped Over The Lazy Dogs 0123456789.";
			const int scroll = ((counter - 200) >> 1) %  (sizeof(banner) - LED_DIGITS);
			led7seg.showText(banner + scroll);
		}
#endif
		else if (counter < 600)
		{
			// Display encoder value as signed -x.yy
			led7seg.showDecimal(encoder >> 2, 2);
		}
		else if (counter < 800)
		{
			// Display counter in hex dddd
			led7seg.showHex(counter);
		}
		else if (counter < 1000)
		{
			// Display counter as unsigned xxx.y
			led7seg.showNumber(counter, 1);
		}
		else
		{
			// Reset to 0 after 100 seconds
			counter = 0;
		}
	}
//	led7seg.refreshDigits(); // Refresh/multiplex display
	led7seg.refreshSegments(); // Refresh/multiplex display
}

//
// This example demonstrates the functionality of the LED7Seg library
// optimized for a 3 or 4 digit display.
//
// It creates a PWM output on pins D10,D11 compatible with Talon, TalonSRX 
// and SparkMax FRC motor controllers
//
// It monitors the battery voltage on A7:
//            2.2K             1.0K
//  VIN  ----/\/\/\-----o-----/\/\/\-------|>
//                      |
//                      A7
// User input is set using a quadrature rotary encoder on D2-D3
// Power is -1.00..+1.00 corresponding to:
//     1.0 msec (full rev)
//     1.5 msec (stop)
//     2.0 msec (full fwd)
//
#include "LED7Seg.h"
#include <Servo.h>

#define LEDFONT

#define MODE_PIN        0     // RXD
#define ENC_PIN         2     // Pins D2 & D3
#define PWM_OUT         10

#define MIN_POWER       -100  // -100%
#define MAX_POWER       +100  // +100%

SevSeg led7seg;        //Instantiate LED7Seg object
#define LED_DIGITS      4

Servo pwmOut;

//           MOD         ENA ENB D1   A   F  D2  D3   B
//      +-o---o---o---o---o---o---o---o---o---o---o---o---o---o---o-+
//	    |TX0 RX1 RST GND D2  D3| D4  D5  D6  D7  D8  D9| D10 D11 D12|
//	    |                      |     Arduino Nano      |            |
//      |VIN GND RST 5V  A7  A6| A5  A4  A3  A2  A1  A0|REF 3V3 D13 |
//      +-o---o---o---o---o---o---o---o---o---o---o---o---o---o---o-+
//                       AIN BAT  E   D  DP   C   G  D4
const byte ledPins[] = { /*segA-F+DP=*/ 5, 9, A2, A4, A5, 6, A1, A3, /*dig1-4=*/ 4, 7, 8, A0};

void setup()
{
    Serial.begin(57600);
    pinMode(MODE_PIN, INPUT_PULLUP);

    analogReference(DEFAULT);
    led7seg.begin(COMMON_CATHODE, LED_DIGITS, ledPins);
	  pinMode(LED_BUILTIN, OUTPUT);
    pinMode(A6, INPUT_PULLUP);
    pinMode(A7, INPUT_PULLUP);
    pinMode(2, INPUT);
    pinMode(3, INPUT);
    digitalWrite(2, HIGH);    // Turn on pullup for D2
    digitalWrite(3, HIGH);    // Turn on pullup for D3

    pwmOut.attach(PWM_OUT);
}

uint8_t old_AB;
