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;
}
}
}
}
