[Terk-developer] embed/src/diagnostics checkCFG.h, NONE, 1.1 displayID.h, NONE, 1.1 qwerkmenu.h, NONE, 1.1 testmotor.h, NONE, 1.1 testservo.h, NONE, 1.1 checkCFG.cxx, NONE, 1.1 displayID.cxx, NONE, 1.1 testmotor.cxx, NONE, 1.1 testservo.cxx, NONE, 1.1

[Ivan D. <idr...@us...>]

Update of /cvsroot/terk/embed/src/diagnostics
In directory sc8-pr-cvs10.sourceforge.net:/tmp/cvs-serv19949/src/diagnostics

Added Files:
	checkCFG.h displayID.h qwerkmenu.h testmotor.h testservo.h 
	checkCFG.cxx displayID.cxx testmotor.cxx testservo.cxx 
Log Message:
recovering: adding more files

--- NEW FILE: qwerkmenu.h ---
#ifndef _QWERKMENU_H
#define _QWERKMENU_H

#define CFG_PERIOD  2000

#define PERIOD      1500   // time for digit to be displayed (msec)
#define EMPTY       0      // "all LEDs off" mask
#define FULL        255    // "all LEDs on " mask

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "qwerkhw.h"
#include "timeutil.h"
#include "displayID.h"
#include "testmotor.h"
#include "testservo.h"

class CQwerkMenu
{
    public:

        CQwerkMenu()
        {
            m_pQwerk = new CQwerkHardware();

            cfgState     = false;
            cfgStatePrev = false;
            cfgCount     = 0;

            execute = false;

            exit = false;

            gettimeofday(&cfgTimeoutTime, NULL);
        }

        void CQwerkMenu::LaunchMenu()
        {
            exit = false;

            FlashLEDs(7, 60);

            while(!exit)
            {
                UpdateCFG();

                if (execute)
                    LaunchMenuItem(cfgCount-1);
            }
        }



    private:
        CQwerkHardware *m_pQwerk;     //parent

        bool cfgState;
        bool cfgStatePrev;
        int  cfgCount;
        bool execute;
        bool exit;

        struct timeval cfgLastPressTime;
        struct timeval cfgTimeoutTime;
        struct timeval curTime;

        void FlashLEDs(int leds, int times)
        {
            int i, j;

            for (i=0; i<times; i++)
            {
                for (j=0; j<=leds; j++)
                    m_pQwerk->SetLED(j, true);

                Wait(10);

                for (j=0; j<=leds; j++)
                    m_pQwerk->SetLED(j, false);

                Wait(10);
            }
        }

        void Wait (int msec)
        {
            usleep(msec*1000);
        }

        void UpdateCFG()
        {
            // ***** update button states ************

            cfgStatePrev = cfgState;
            cfgState     = m_pQwerk->GetConfigSwitch();

            // ***** CFG press event *****************

            if (cfgState && !cfgStatePrev)
            {
                cfgCount++;
                m_pQwerk->SetLED(cfgCount-1, true);

                //add time to timeout value
                gettimeofday(&cfgTimeoutTime, NULL);
                TIMEVAL_ADD_MSEC(&cfgTimeoutTime, CFG_PERIOD);
            }

            // ***** check for timeout ***************

            else
            {
                gettimeofday(&curTime, NULL);
                if (TIMEVAL_CMP(&curTime, &cfgTimeoutTime) > 0)
                if (cfgCount > 0)
                    execute = true;
            }
        }

        void LaunchMenuItem(int n)
        {
            // ***** reset counters *****************

            execute = false;
            cfgCount = 0;

            // ***** display menu item selection ****

            FlashLEDs(n, 20);

            // ***** launch programs *****************

            if      (n == 0) DisplayRobotID();
            else if (n == 1) TestMotorsPWM();
            else if (n == 2) TestServos();

            else if (n >= 7)
            {
                m_pQwerk->SetLEDs(EMPTY);
                exit = true;
                return;
            }

            // ***** display program finished *******

            FlashLEDs(7, 20);
        }
};

#endif

--- NEW FILE: checkCFG.h ---
#ifndef _CHECKCFG_H

#define _CHECKCFG_H


#endif



--- NEW FILE: testmotor.h ---
#ifndef __TESTMOTOR_H__

#define __TESTMOTOR_H__

void TestMotorsPWM();
void TestMotorsMove();

#endif



--- NEW FILE: displayID.h ---
#ifndef __DISPLAY_ID_H__

#define __DISPLAY_ID_H__

#define DIGITS  28					// number of SN digits
#define KEY 	0x9A77013E
#define MOD 	0x00000FFD
#define FILE	"/opt/robotid.txt"

#define PERIOD  2000				// time for digit to be displayed (msec)
#define EMPTY   0					// "all LEDs off" mask
#define FULL    255					// "all LEDs on " mask

void DisplayRobotID();
void Encode (unsigned long name, unsigned long pass, int* id);
void PrintID(int* id);
int  GetOctalDigit(unsigned long number, unsigned int pos);
void ReadFile (unsigned long * name, unsigned long * pass);
void Wait(int msec);

// ID: N5 P7 C2 P3 P11 N9 C0 P2 P6 N8 N11 N10 P0 C1 P10 C3 N7 N0 P5 N3 P8 P9 N2 N6 P1 P4 N1 N4

#endif



--- NEW FILE: testservo.cxx ---
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "rcservo.h"
#include "qwerkhw.h"

void TestServos()
{
    printf("testing servos:\n");

    CQwerkHardware qwerk(NULL);
    CRCServo qwerkServos;
    int i, s;

    // ************* enable

    qwerkServos.Enable();

    // ************* set

    for (s = 0; s < 4; s++)
    {
        printf("\ttesting servos %d %d %d %d ...\n",
                        s*4+0, s*4+1, s*4+2, s*4+3);

        // display progress on LEDs

        qwerk.SetLED(s*2,   true);
        qwerk.SetLED(s*2+1, true);

        // rotate 128 -> 255

        for (i = 128; i <= 255; i++)
        {
            qwerkServos.SetPosition(s*4+0, i);
            qwerkServos.SetPosition(s*4+1, i);
            qwerkServos.SetPosition(s*4+2, i);
            qwerkServos.SetPosition(s*4+3, i);
            usleep(2000);
        }

        // rotate 255 -> 0

        for (i = 255; i >= 0; i--)
        {
            qwerkServos.SetPosition(s*4+0, i);
            qwerkServos.SetPosition(s*4+1, i);
            qwerkServos.SetPosition(s*4+2, i);
            qwerkServos.SetPosition(s*4+3, i);
            usleep(2000);
        }

        // rotate 0 -> 128

        for (i = 0; i <= 128; i++)
        {
            qwerkServos.SetPosition(s*4+0, i);
            qwerkServos.SetPosition(s*4+1, i);
            qwerkServos.SetPosition(s*4+2, i);
            qwerkServos.SetPosition(s*4+3, i);
            usleep(2000);
        }
    }

    // ************* disable

    qwerk.SetLEDs(0);

    qwerkServos.Disable();
}


--- NEW FILE: displayID.cxx ---
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <string>

using namespace std;

#include "qwerkhw.h"
#include "displayID.h"

void DisplayRobotID()
{
	// ***************************** variables

    CQwerkHardware hardware(NULL);	// the qwerk board

	unsigned long name;				// username
	unsigned long pass;				// password
	int  id[DIGITS];				// array of octal ID digits
	int* p_id = id;					// pointer

	unsigned char stateMask;        // LED state mask
  
	// ***************************** read file
 
	ReadFile(&name, &pass);
 
	// ***************************** encode
 
	Encode(name, pass, p_id);
	cout << endl;
 
    // ***************************** loop, displaying serial number

    Wait(PERIOD);

    for (int i=0; i<DIGITS; i++)
    {
        // ************************* set LEDs to mask
        
		stateMask = 1<<id[i];
		hardware.SetLEDs(stateMask);
        Wait(PERIOD*0.7);

        // ************************* set LEDs to empty
        
		hardware.SetLEDs(EMPTY);
        Wait(PERIOD*0.3);
	  }
}
 
void ReadFile (unsigned long * name, unsigned long * pass)
{
	ifstream::pos_type size;

    // ***************************** create stream

	ifstream file (FILE);

	if (file.is_open())
	{
		// ************************* read data

		file >> hex >> *name;
		file >> hex >> *pass;

		cout << "Robot name & pass loaded" << endl;

		// ************************* close

		file.close();
	}

	else 
		cout << "Unable to open file" << endl;
}
 
void Encode (unsigned long name, unsigned long pass, int* id)
{
	unsigned int csum;				// checksum
 
	// ***************************** encode username, password, and checksum
 
	printf("username: \t\t%08X\n", name);
	printf("password: \t\t%08X\n", pass);
 
	csum = (name + pass)%MOD;
	name = name ^ KEY;
	pass = pass ^ KEY;
 
	printf("checksum:\t%08X\n\n", csum);
 
	// ***************************** encode

	id[ 0] = GetOctalDigit(name,  5); //  0
	id[ 1] = GetOctalDigit(pass,  7); //  1
	id[ 2] = GetOctalDigit(csum,  2); //  2
	id[ 3] = GetOctalDigit(pass,  3); //  3
	id[ 4] = GetOctalDigit(pass, 11); //  4
 
	id[ 5] = GetOctalDigit(name,  9); //  5
	id[ 6] = GetOctalDigit(csum,  0); //  6
	id[ 7] = GetOctalDigit(pass,  2); //  7
	id[ 8] = GetOctalDigit(pass,  6); //  8
	id[ 9] = GetOctalDigit(name,  8); //  9
 
	id[10] = GetOctalDigit(name, 11); // 10
	id[11] = GetOctalDigit(name, 10); // 11
	id[12] = GetOctalDigit(pass,  0); // 12
	id[13] = GetOctalDigit(csum,  1); // 13
	id[14] = GetOctalDigit(pass, 10); // 14
 
	id[15] = GetOctalDigit(csum,  3); // 15
	id[16] = GetOctalDigit(name,  7); // 16
	id[17] = GetOctalDigit(name,  0); // 17
	id[18] = GetOctalDigit(pass,  5); // 18
	id[19] = GetOctalDigit(name,  3); // 19
 
	id[20] = GetOctalDigit(pass,  8); // 20
	id[21] = GetOctalDigit(pass,  9); // 21
	id[22] = GetOctalDigit(name,  2); // 22
	id[23] = GetOctalDigit(name,  6); // 23
	id[24] = GetOctalDigit(pass,  1); // 24
 
	id[25] = GetOctalDigit(pass,  4); // 25
	id[26] = GetOctalDigit(name,  1); // 26
	id[27] = GetOctalDigit(name,  4); // 27
 
	// **************************** print ID
 
	PrintID(id);
 
}
 
void PrintID(int* id)
{
	cout << "ENCODED ID octal: \t";
 
	for (int i = 0; i < 28; i++)
		cout << id[i];
 
	cout << endl;
}
 
int GetOctalDigit(unsigned long number, unsigned int pos)
{
	return (unsigned int)(number / pow(8, pos)) % 8;
}
 

void Wait (int msec)
{
    usleep(msec*1000);
}

--- NEW FILE: testservo.h ---
#ifndef __TESTSERVO_H__

#define __TESTSERVO_H__

void TestServos();

#endif



--- NEW FILE: checkCFG.cxx ---
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "qwerkhw.h"
#include "qwerkmenu.h"

int main()
{
    CQwerkHardware  qwerk(NULL);
    CQwerkMenu      m;

    if (qwerk.GetConfigSwitch())
        m.LaunchMenu();

    return 0;
}

--- NEW FILE: testmotor.cxx ---
#include <stdio.h>
#include <unistd.h>
#include "qemotortraj.h"
#include "qwerkhw.h"
#include "qegpioint.h"

void TestMotorsPWM()
{
    printf("testing motors PWM:\n");

    CQwerkHardware qwerk(NULL);
    CQEMotorTraj motor(NULL, 0, 1, 2, 3);

    int i, m;

    for (m = 0; m < 4; m++)
    {
        printf("\ttesting motor %d...\n", m);

        for (i = 0; i <= 255; i++)
        {
            qwerk.SetLED(i/32, true);
            motor.SetPWM(m, i);
            usleep(2000);
        }

        for (i = 255; i >= 0; i--)
        {
            qwerk.SetLED(i/32, false);
            motor.SetPWM(m, i);
            usleep(2000);
        }

        for (i = 0; i >= -255; i--)
        {
            qwerk.SetLED(-i/32, true);
            motor.SetPWM(m, i);
            usleep(2000);
        }

        for (i = -255; i <= 0; i++)
        {
            qwerk.SetLED(-i/32, false);
            motor.SetPWM(m, i);
            usleep(2000);
        }

        qwerk.SetLEDs(0);
    }
}

void TestMotorsMove()
{
    CQEMotorTraj motor(NULL, 0, 1, 2, 3);

    motor.SetPIDVGains(0, 100, 0, 0, 0);

    printf("running\n");

    motor.Move(0,  40000, 20000, 10000);
    while(!motor.Done(0));

    motor.Move(0, -20000, 20000, 10000);
    while(!motor.Done(0));

}


/*
    motor.SetPIDVGains(0, 300, 0, 50, 0);
    motor.SetPIDVGains(1, 300, 0, 50, 0);

    motor.Hold(0, true);
    motor.Hold(1, true);

    motor.SetPosition(0, 10000);
    motor.SetPosition(1,  5000);
*/
    //motor.MoveVelocity(0, 10000, 1);