lcdtask.c

AVR单片机做的两轮自平衡机器人

#define ENABLE_BIT_DEFINITIONS
#include <avr/eeprom.h>
#include "avrx-io.h"
#include "avrx.h"
#include "hardware.h"
#include "MotorTask.h"
#include "lcdio.h"
#include "adc_driver.h"
#include "LcdTask.h"
#include "Gyro_eeprom.h"
#include "SensorTask.h"

TimerControlBlock LcdTimer;
Mutex StartFlag;

static char FLASH hello[]   = "Gyrobot 0.1";
static char FLASH sAngle[]  = "A:";
static char FLASH sVoltage[]= "V:";
static char FLASH stX[]     = "X:";
static char FLASH stR[]     = " R:";
static char FLASH stY[]     = " Y:";
static char FLASH stL[]     = "L:";
static char FLASH stT[]     = "T:";
static char FLASH sON[]     = "On ";
static char FLASH sOFF[]    = "Off";
static char FLASH sReset[]  = "EEPROM Corrupt: ";
static char FLASH sReset1[] = "Reset";
static char FLASH sDone[]   = "Done!";
static char FLASH sHalt[]   = "Fatal Error ";
static char FLASH sADC[]    = "AD Channel";
static char FLASH sBehavior[] = "Task: ";
static char FLASH sUMBLeft[]= "UMBMark Left";
static char FLASH sUMBRight[]="UMBMark Right";
static char FLASH sGyro[]   = "GyroOffset:";
static char FLASH sAgain[]  = "AngleGain:";
static char FLASH sKangle[] = "Kangle:";
static char FLASH sKrate[]  = "Krate:";
static char FLASH sKiangle[]= "Kiangle:";
static char FLASH sKp[]     = "Kp:";
static char FLASH sKd[]     = "Kd:";
static char FLASH sKi[]     = "Ki:";
static char FLASH sVsp[]    = "Velocity SP:";
static char FLASH sVelocity[]="Velocity:";

void lcd_PutUDec(unsigned val)
{
    char i, lz = 0;

    for (i = 0; val >= 1000; i++)
        val -= 1000;
    if (i || lz)
    {
        lcd_PutHexChar(i);
        lz = 1;
    }
    for (i = 0; val >= 100; i++)
        val -= 100;
    if (i || lz)
    {
        lcd_PutHexChar(i);
        lz = 1;
    }
    for (i = 0; val >= 10; i++)
        val -= 10;
    if (i || lz)
    {
        lcd_PutHexChar(i);
        lz = 1;
    }
    lcd_PutHexChar(val);
}

int lcd_PrintSign(int val)
{
    if (val < 0)
    {
        lcd_put_char('-');
        val = -val;
    }
    else
        lcd_put_char(' ');
    return val;
}

void lcd_PutIDec(int val)
{
    val = lcd_PrintSign(val);
    lcd_PutUDec(val);
}

void lcd_PutUTenths(int val)
{
    lcd_PutUDec(val/10);
    lcd_put_char('.');
    lcd_PutHexChar(val%10);
}

void lcd_PutITenths(int val)
{
    val = lcd_PrintSign(val);
    lcd_PutUTenths(val);
}

void lcd_PutTime(int val)
{
    char i;

    for (i = 0; val >= 600; i++)
        val -= 600;
    lcd_PutHexChar(i);
    lcd_put_char(':');
    lcd_PutUTenths(val);
}

void ByteEdit(char opt, const char *addr, const char *str)
{
    char bTemp = eeprom_read_byte((unsigned)addr) + opt;
    if (opt)
    {
        eeprom_write_byte((unsigned)addr, bTemp);
        WriteChecksum();
    }
    lcd_putstr(str);
    lcd_PutIDec(bTemp);
}

void WordEdit(char opt, const short *addr, const char *str)
{
    unsigned wTemp = eeprom_read_word((unsigned)addr) + opt;

    if (opt)
    {
        eeprom_ww((unsigned)addr, wTemp);
        WriteChecksum();
    }
    lcd_putstr(str);
    lcd_PutIDec(wTemp);
}

void HaltError(char ploc)
{
//    StopMotion();
    lcd_wr_inst(LCD_CLEAR);
    lcd_wr_inst(LCD_HOME);
    lcd_putstr(sHalt);
    lcd_PutHexByte(ploc);
    AvrXHalt();
}

/*
    LcdTask updates the LCD display 5 times/second.
    Run at low priority since it is slow

*/
AVRX_GCC_TASKDEF(LcdTask, 20, 8)
{
    char    update  = 0,    // Can you believe that only 7 register
            option  = 0,    // are used for all this?
            LCDmode = 0,
            PrevLCDmode = 1,
            sw_current  = 0,
            sw_stable   = 0,
            channel     = 0,
            sw_changed,
            sw_tmp;

    ADC_Init();

    lcd_4_init();
    lcd_wr_inst(LCD_FUNC | LCD_N);
    lcd_wr_inst(LCD_CLEAR);
    lcd_wr_inst(LCD_HOME);
    lcd_wr_inst(LCD_DISPLAY | LCD_D | LCD_C);
    lcd_putstr(hello);
    AvrXDelay(&LcdTimer, MILLISECOND(1000));

    LED_ENABLE();     // lcd_4_init clobbers this.

    while(1)
    {
        AvrXStartTimer(&LcdTimer, MILLISECOND(40));

        while ((++update & 0x7) == 7)       // True every 8 cycles (320ms)
        {
            outp(inp(PORTB) ^ BV(PB4), PORTB);// ping-pong the LED

            if (ValidateEEPROM())
            {
                lcd_wr_inst(LCD_CLEAR);
                lcd_putstr(sReset);
                lcd_setcursor(LCD_LINE2);
                lcd_putstr(sReset1);
                if (option)
                {
                    lcd_putstr(sDone);
                    option = 0;
                    ResetEEPROM();
                    lcd_wr_inst(LCD_CLEAR);
                }
                continue;
            }

            if ((LCDmode != PrevLCDmode) || option)
            {
                lcd_wr_inst(LCD_CLEAR);      // Don't clear screen unless mode changes
                PrevLCDmode = LCDmode;
            }
            lcd_wr_inst(LCD_HOME);
            lcd_put_char('a');
            lcd_PutITenths(iTenthsDegree);
            lcd_setcursor(LCD_LINE1+10);
            lcd_put_char('+');
            lcd_PutUTenths((GetADC(VBATT)*20)/93);
            lcd_setcursor(LCD_LINE2);

            switch(LCDmode)
            {
                case D_STATUS:
                    lcd_putstr(stL);
                    lcd_PutHexWord(Left.Encoder);
                    lcd_putstr(stR);
                    lcd_PutHexWord(Right.Encoder);
                break;

                case D_VELOCITY:
                    lcd_putstr(sVelocity);
                    lcd_PutIDec(velocity);
                    break;

                case D_POSITION:
                    lcd_putstr(stX);
                    lcd_PutITenths(iX);         // Scale for 10th of an inch
                    lcd_putstr(stY);
                    lcd_PutITenths(iY);
                    lcd_setcursor(LCD_LINE3);
                    lcd_putstr(stT);
                    lcd_PutIDec(iTheta);
                    break;

                case D_ADC:
                    channel += option;
                    channel &= 0x7;             // 3 bits
                    lcd_putstr(sADC);
                    lcd_PutUDec(channel);
                    lcd_PutIDec(GetADC(channel));
                    break;

                case D_ANGLEGAIN:
                    WordEdit(option, &eeiAngleGain, sAgain);
                    break;
                case D_GYROOFFSET:
                    WordEdit(option, &eeiGyroOffset, sGyro);
                    break;
                case D_KANGLE:
                    WordEdit(option, &eeKangle, sKangle);
                    break;
                case D_KRATE:
                    WordEdit(option, &eeKrate, sKrate);
                    break;
                case D_KIANGLE:
                    WordEdit(option, &eeKiangle, sKiangle);
                    break;
                case D_KP:
                    WordEdit(option, &eeKp, sKp);
                    break;
                case D_KD:
                    WordEdit(option, &eeKd, sKd);
                    break;
                case D_KI:
                    WordEdit(option, &eeKi, sKi);
                    break;
                case D_VSP:
                    if (option)
                    {
                        VelocitySp += option;
                    }
                    lcd_putstr(sVsp);
                    lcd_PutIDec(VelocitySp);
                    break;
                default:
                    LCDmode = 0;    // Wrap forward
                    break;
                case -1:            // Wrap reverse
                    LCDmode = D_LAST;
                    break;
            }
            option = 0;             // Option only last one cycle.
        }                           // End of while (update)

        // Debounce switch input

        sw_tmp      = sw_current;
        sw_current  = LCD_SWITCHES();
        sw_tmp      = sw_tmp ^ sw_current;
        sw_changed  = (sw_current ^ sw_stable) & ~sw_tmp;
        sw_stable   = (sw_stable & sw_tmp) | (sw_current & ~sw_tmp);

        // Now parse the debounced switch information
        // Switches recently "opened" will be stable & changed.

        switch(sw_stable & sw_changed)
        {
            case BV(SW_UP):
                --option;
                break;
            case BV(SW_DOWN):
                ++option;
                break;
            case BV(SW_ITEM):
                LCDmode++;
                break;
            case BV(SW_HALT):
                LCDmode--;
                break;
        }

        AvrXWaitTimer(&LcdTimer);
    }   // end while()
}