void MotorInit(void){ MT_Input_Init(&motorR,&motorL); EncoderInit(&encoderR,&encoderL); Robot.volt = 7.5; Robot.refPwmL = 0; Robot.refPwmR = 0; Robot.vR = 0; Robot.vL = 0; Robot.wL = 0; Robot.wR = 0; Robot.newCom = false; Robot.sendOK = false; Robot.errSumL = 0; Robot.errSumR = 0; RobotNew.a = 1.0; returnData[12] = '\n'; robotSetPos(0,0,0); MotionTimInit(&motionTimer); SerialUploadTimInit(&serialUpTimer); max_Vel.tang_max = TANG_VEL_MAX; max_Vel.ang_max = ANG_VEL_MAX; PID.Ki = I; PID.Kp = P; Stop(&motorL, &motorR); }
/** *@brief Initial *@param None; *@retval None */ void Initial() { SetParam(); NVIC_Configuration(); //中断初始化 delay_init(); //延时初始化 uart_init(115200); //串口初始化 EncoderInit(&PendBarEncoder); //编码器初始化 }
int main(void) { //Code geoptimaliseerd voor bordje 3 //###1### //Initialize subsystems ClockInit(); //Initialize system clock (16 MHz) USARTInit(); //Initialize USART and bind to stdout,stdin AnalogInit(); //Initialize ADC AccInit(); //Initialize accelerometer system LEDInit(); //Initialize LEDs SwitchInit(); //Initialize switches EncoderInit(); //Initialize encoder SpeakerInit(); //Initialize speaker system //Enable interrupts PMIC.CTRL|=0b00000111; //Enable low, medium, high priority interrupts SREG|=0b10000000; //Globale interrupt enable //###2### //Print the digits 0 to 9 5x on terminal device //Reason this section didn't work: de variabelen 'a' is globaal gedefinieerd. In het subprogramma 'SimpleFunction()' wordt //de variabelen 'a' gebruikt. Na dit subprogramma is de waarde van 'a' gelijk aan 10 en zal de lus (a<5) onderbroken worden. for (a=0; a<5; a++) { SimpleFunction(); printf ("\r\n"); } //###3### //Main program loop a=0; ledOn=0b00001000; //Beeps(); //Genereer 3 beeps van 500hz, 1000hz, 1500hz (500 ms) setNotes(); //Stel een reeks van noten in playNextNote(); //Speel de eerste noot uit de reeks while(1) { LoopLicht(); //LoopLicht sequentie AccLezen(); //Accelerometer lezen printf("$SWITCH %d\r\n", SwitchGet()); //Switchwaarde over USARTD: 1: center, 2: rechts, 4: beneden, 8: links, 16: boven printf("$ACCRAW %d %d %d\r\n", RawAccX, RawAccY, RawAccZ); //Ongecalibreerde waardes accelerometer x, y, z over USARTD printf("$ACC__ %d %d %d\r\n", AccGetXAxis(RawAccX), AccGetYAxis(RawAccY), AccGetZAxis(RawAccZ)); //Gecalibreerde waardes accelerometer x, y, z over USARTD printf("$ENC__ %d\r\n", EncoderGetPos()); //printf ("Counter:%d\r\n",a); //printf("%c", 0x55); //Stuur de waarde 0x55 uit (0b01010101) //a++; _delay_ms(20); } }
int main(void) { uint32_t current_time = 0; SystemInit(); GpioInit(); //AdcInit(); TimerInit(0, 1ul * _millisecond); CallbackRegister(MotorHandler, 10ul * _millisecond); CallbackEnable(MotorHandler); TimerEnable(0); EncoderInit(); MotorInit(); MotorStart(); I2cInit(SENSOR_BUS); current_time = now; while(now < current_time + 1000); // center the wheels if they aren't already set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70); set_motor_position(MOTOR_BACK_FRONT, BF_CENTER, 72); while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING); current_time = now; while(now < current_time + 5000); // offset one motor set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70); set_motor_position(MOTOR_BACK_FRONT, BF_CENTER + 50, 72); while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING); current_time = now; while(now < current_time + 1000); while (1) { if (get_ir_sen()) { set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER + 50, -70); set_motor_position(MOTOR_BACK_FRONT, BF_CENTER, -72); while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING); } else { set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70); set_motor_position(MOTOR_BACK_FRONT, BF_CENTER + 50, 72); while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING); } } return 0; }
void ChangePwd() { char newpass[255] = {0}; int action = DialogBoxParam(g_hInst, MAKEINTRESOURCE(IDD_CHANGEPASS), NULL, (DLGPROC)DlgChangePass, (LPARAM)newpass); if(action == IDCANCEL || (action == IDOK && !strlen(newpass))) return; EnterCriticalSection(&csDbAccess); DecodeAll(); CryptoEngine->FreeKey(key); if(action == IDREMOVE){ WritePlainHeader(); bEncoding = 0; CryptoEngine = NULL; DBWriteContactSettingWord(NULL, "SecureMMAP", "CryptoModule", 0); zero_fill(encryptKey, sizeof encryptKey); if (gl_bUnicodeAwareCore) xModifyMenu(hSetPwdMenu, 0, LPGENT("Set Password"), 0); else xModifyMenu(hSetPwdMenu, 0, (TCHAR*) LPGEN("Set Password"), 0); //ugly hack } if(action == IDOK){ strcpy(encryptKey, newpass); encryptKeyLength = strlen(newpass); bEncoding = 1; EncoderInit(); EncodeAll(); WriteCryptHeader(); } zero_fill(newpass, sizeof newpass); LeaveCriticalSection(&csDbAccess); }
int CheckPassword(WORD checkWord, char * szDBName) { WORD ver; int res; if(bCheckingPass) return 0; bCheckingPass = 1; { int i; int Found = 0; for(i = 0; i < ModulesCount; i++){ if(dbHeader.cryptorUID == Modules[i]->cryptor->uid){ CryptoEngine = Modules[i]->cryptor; Found = 1; break; } } if(!Found){ MessageBoxA(0, "Sorry, but your database encrypted with unknown module", "Error", MB_OK); bCheckingPass = 0; return 0; } } while(1){ res = DialogBoxParam(g_hInst, MAKEINTRESOURCE(IDD_LOGIN), NULL, (DLGPROC)DlgStdInProc, (LPARAM)szDBName); if(res == IDCANCEL) { wrongPass = 0; bCheckingPass = 0; return 0; } if(encryptKeyLength < 1) continue; EncoderInit(); DecodeCopyMemory(&ver, &checkWord, sizeof(checkWord)); if(ver == 0x5195) { wrongPass = 0; bCheckingPass = 0; return 1; } wrongPass++; } bCheckingPass = 0; }
MAIN void main(void) { Init(); // Enable odometry LED_CONFIGURE_ODOMETRY(); // Configure auto measurment mode SensorInit(); SensorConfigAutomode(am_odo); // Setup encoder control EncoderInit(); EncoderMovementReset(); // Initialize 1kHz tick timer Timer0Init(); Timer0IntEnable(); // Set drive direction MotorDir(FWD, FWD); for (;;) { // Circle with approx. 10cm diameter ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { EncoderMovementSetSpeed(INNER_SPEED, OUTER_SPEED(100)); } msleep(5000); // Circle with approx. 25cm diameter ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { EncoderMovementSetSpeed(INNER_SPEED, OUTER_SPEED(250)); } msleep(3000); // Tightening spiral for (uint8_t i = 250; i >= 80; i -= 10) { ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { EncoderMovementSetSpeed(INNER_SPEED, OUTER_SPEED(i)); } msleep(500); } // Straight line ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { EncoderMovementSetSpeed(125, 125); } msleep(3000); } }
void EncryptDB() { int action = 0; if(bEncProcess) return; if(memcmp(dbHeader.signature, &dbSignatureSecured, sizeof(dbHeader.signature)) == 0){ if (gl_bUnicodeAwareCore) MessageBox(0, TranslateT("DB is already secured!"), TranslateT("Error"), MB_OK); else MessageBoxA(0, Translate("DB is already secured!"), Translate("Error"), MB_OK); return; } if(SelectEncoder()){ return; } bEncProcess = 1; action = DialogBoxParam(g_hInst, MAKEINTRESOURCE(IDD_NEWPASS), NULL, (DLGPROC)DlgStdNewPass, (LPARAM)NULL); if(action != IDOK || !strlen(encryptKey)){ bEncProcess = 0; DBWriteContactSettingByte(NULL, "SecureMMAP", "CryptoModule", 0); return; } EnterCriticalSection(&csDbAccess); bEncoding = 1; EncoderInit(); EncodeAll(); LeaveCriticalSection(&csDbAccess); WriteCryptHeader(); if (gl_bUnicodeAwareCore) xModifyMenu(hSetPwdMenu, 0, LPGENT("Change Password"), 0); else xModifyMenu(hSetPwdMenu, 0, (TCHAR*) LPGEN ("Change Password"), 0); //ugly hack bEncProcess = 0; }
void RecryptDB() { EnterCriticalSection(&csDbAccess); DecodeAll(); CryptoEngine->FreeKey(key); SelectEncoder(); bEncoding = 1; EncoderInit(); EncodeAll(); WriteCryptHeader(); LeaveCriticalSection(&csDbAccess); }
/******************************************************************************************************** Function Name: main Description : Inputs : None Outputs : None Notes : Revision : ********************************************************************************************************/ int main(void) { SystemInit(); //ϵͳʱÖÓ³õʼ»¯ InputIOInit(); //Êý×ÖÊäÈë³õʼ»¯ OutputIOInit();//Êý×ÖÊä³ö³õʼ»¯ SensorInit(); //Ä£ÄâÊäÈë³õʼ»¯ ZLG7290Init(); //¼üÅ̳õʼ»¯ MotorInit(); //µç»ú³õʼ»¯ ServoInit(); //¶æ»ú³õʼ»¯ EncoderInit(); //±àÂëÆ÷³õʼ»¯ /* V_PIDInit(); //µç»ú±Õ»·Êä³ö */ SystemStart(); //UsartInit(); //´®¿Ú³õʼ»¯ }
/** Configures the board hardware and chip peripherals for the demo's functionality. */ void SetupHardware(void) { /* Disable watchdog if enabled by bootloader/fuses */ MCUSR &= ~(1 << WDRF); wdt_disable(); /* Disable clock division */ clock_prescale_set(clock_div_1); /* Configure all button pins to use internal pullup */ PORTD |= (1<<7) | (1<<4) | (1<<2) | (1<<0) | (1<<6) | (1<<1); PORTE |= (1<<2); PORTB |= (1<<0) | (1<<4) | (1<<5) | (1<<7); /* Subsystem Initialization */ EncoderInit(); DebounceInit(); LedInit(); USB_Init(); }
void bcr_initBarcodeReader(void) { EncoderInit(); MotorDir(FWD,FWD); FrontLED(ON); lineCounter = 0; rawCounter = 0; latestLineData = 0; unsigned int tmp[2]; util_getStableLineData(tmp); tolFactor = getToleranceValue(tmp[LEFT] + tmp[RIGHT]); int i; util_pauseInterrupts(); for(i = 0; i < (M_RAW_DATAPOINTS * M_WINDOW_SIZE); i++) { measureDataPoint(); Msleep(5); } ts_init(); ts_addFunction(&measureDataPoint, M_SAMPLING_FREQ); sei(); }
static void MOTtask(void *pvParameters) { int commsStatsCount = commStatsSecs; int initReply = 0; bool initialized = false; //start subsystem tasks initReply += SysLogInit(); initReply += PubSubInit(); initReply += NotificationsInit(); initReply += UARTBrokerInit(); initReply += PidInit(); //Controls the motor PWM initReply += EncoderInit(); //Measures motor speed //initReply += AmpsInit(); //Measures motor current while ((motQueue = psNewPubSubQueue(MOT_TASK_QUEUE_LENGTH)) == NULL) { SetCondition(MOT_INIT_ERROR); LogError( "Motor Task Q"); initReply = -1; } if (initReply < 0) { SetCondition(MOT_INIT_ERROR); DebugPrint("MOT Init Error"); while (1) { vTaskDelay(100); PORTToggleBits(USER_LED_IOPORT, USER_LED_BIT); } } CancelCondition(MOT_INIT_ERROR); CancelCondition(MOTORS_INHIBIT); CancelCondition(MOTORS_BUSY); CancelCondition(MOT_ANALOG_ERROR); CancelCondition(MOTORS_ERRORS); CancelCondition(MOT_MCP_COMMS_ERRORS); CancelCondition(MOT_MCP_COMMS_CONGESTION); { psMessage_t msg; psInitPublish(msg, SS_ONLINE); strcpy(msg.responsePayload.subsystem, "MOT"); msg.responsePayload.flags = RESPONSE_FIRST_TIME; psSendMessage(msg); } DebugPrint("MOT Up"); for (;;) { //wait for a message if (xQueueReceive(motQueue, &motRxMsg, portMAX_DELAY) == pdTRUE) { switch (motRxMsg.header.messageType) { case TICK_1S: powerState = motRxMsg.tickPayload.systemPowerState; if (--commsStatsCount <= 0) { commsStatsCount = commStatsSecs; if (commStats) { UARTSendStats(); } else { UARTResetStats(); } } break; case CONFIG: if (motRxMsg.configPayload.responder == MOTOR_SUBSYSTEM) { DebugPrint("Config message received"); int requestor = motRxMsg.configPayload.requestor; configCount = 0; #define optionmacro(name, var, minV, maxV, def) SendOptionConfig(name, &var, minV, maxV, requestor); #include "Options.h" #undef optionmacro #define settingmacro(name, var, minV, maxV, def) SendSettingConfig(name, &var, minV, maxV, requestor); #include "Settings.h" #undef settingmacro //report Config done psInitPublish(motRxMsg, CONFIG_DONE); motRxMsg.configPayload.requestor = requestor; motRxMsg.configPayload.responder = MOTOR_SUBSYSTEM; motRxMsg.configPayload.count = configCount; psSendMessage(motRxMsg); DebugPrint("Config sent"); } break; case SET_OPTION: //option change DebugPrint("New Option %s = %i", motRxMsg.optionPayload.name, motRxMsg.optionPayload.value); #define optionmacro(n, var, minV, maxV, def) SetOption(&motRxMsg, n, &var, minV, maxV); #include "Options.h" #undef optionmacro break; case NEW_SETTING: //setting change DebugPrint("New Setting %s = %f", motRxMsg.settingPayload.name, motRxMsg.settingPayload.value); #define settingmacro(n, var, minV, maxV, def) NewSetting(&motRxMsg, n, &var, minV, maxV); #include "Settings.h" #undef settingmacro break; case PING_MSG: { psMessage_t msg2; DebugPrint("Ping Msg"); // PORTToggleBits(USER_LED_IOPORT, USER_LED_BIT); psInitPublish(msg2, PING_RESPONSE) strncpy(msg2.responsePayload.subsystem, "MOT", 3); msg2.responsePayload.flags = (initialized ? 0 : RESPONSE_FIRST_TIME); msg2.responsePayload.requestor = motRxMsg.requestPayload.requestor; psSendMessage(msg2); } break; case GEN_STATS: DebugPrint("Send Stats\n"); GenerateRunTimeTaskStats(); GenerateRunTimeSystemStats(); break; default: break; } } } }