|
From: Ivan D. <idr...@us...> - 2006-06-13 20:53:32
|
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); |