void InitHardware(){ #ifdef __MSP430_HAS_PORT1_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P1, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P1, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORT2_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P2, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P2, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORT3_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P3, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P3, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORT4_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P4, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P4, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORT5_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P5, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P5, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORT6_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P6, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P6, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORT7_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P7, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P7, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORT8_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P8, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P8, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORT9_R__ GPIO_setOutputLowOnPin(GPIO_PORT_P9, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_P9, GPIO_ALL); #endif #ifdef __MSP430_HAS_PORTJ_R__ GPIO_setOutputLowOnPin(GPIO_PORT_PJ, GPIO_ALL); GPIO_setAsOutputPin(GPIO_PORT_PJ, GPIO_ALL); #endif InitLED(); InitUserInputButtons(); InitBubbleSensor(); InitOcclusionSensor(); InitReservoirLevelPins(); InitMotor(); }
void MecanumDrive::SetMode( CANJaguar::ControlMode controlMode ) { m_currControlMode = controlMode; if ( m_currControlMode == CANJaguar::kSpeed ) { m_maxOutputSpeed = m_maxSpeedModeRPMs; } else // kPercentVbus { m_maxOutputSpeed = 1; } InitMotor(m_frontLeftMotor); InitMotor(m_frontRightMotor); InitMotor(m_rearLeftMotor); InitMotor(m_rearRightMotor); }
/******************************************************************************* * 函 数 名 :main * 函数功能 :主函数 * 输 入 :无 * 输 出 :无 *******************************************************************************/ void main() { Timer0Init(); InitMotor(); while(1) { SetMotor(); } }
void Test_UseUpBattery(void) { WaitEnable(); InitMotor(); for (;;) { FrontRun(200); Wait(60000); StopRun(); Wait(10000); } }
void MotorControlInit(void) { InitMotor(); mU = 70; last_v = (INT16S)((3927 * 4 * 4) / GetSpeed() * _RTI_P / 4); last_mError = last2_mError = 0; motorTime = lastMotorTime = absoluteTime; }
void MecanumDrive::CheckForRestartedMotor( CANJaguar& motor, const char * strDescription ) { if ( m_currControlMode != CANJaguar::kSpeed ) // kSpeed is the default { if ( motor.GetPowerCycled() ) { Wait(0.10); // Wait 100 ms InitMotor( motor ); char error[256]; sprintf(error, "\n\n>>>>%s %s", strDescription, "Jaguar Power Cycled - re-initializing"); printf(error); setErrorData(error, strlen(error), 100); } } }
void TestMotor(void) { INT8U i; StartTimeBase(); InitMotor(); for (i = 0;i < 3;i++) { FrontRun(100); Wait(1000); StopRun(); Wait(1000); BackRun(100); Wait(1000); } StopRun(); }
int main(void) { int i = 0; for(; i<8; i++) { sensors[i] = 0; } //Needs to be before sei.. don't ask why.. //Also needs a delay from start up.. _delay_ms(1000); InitLCD(); sei(); SETUP_SENSOR_IO(); CLEAR_BIT(DDRE, PE5);//Input pin ENABLE_EXTERNAL_INTERRUPT(INT5);//External Interrupt Request 5 INT5 enabled*/ InitServo(0); InitMotor(); SetupTachometer(); float currentDirection = 0; float targetDirection = 0; bool terminated = false; currentSpeed = 10; tachoPulsesPerSecond = 0; for (;;) { if(updateScreen) { updateScreen = false; ClearScreen(); WriteText("State:",1); WriteText_StartingFrom(status,2,7); WriteText("Lap:",3); WriteText_StartingFrom(lap,4,7); WriteText("Ticker:",5); WriteText_StartingFrom(time,6,7); WriteText("cycle:",7); WriteText_StartingFrom(cyc,8,7); } //Terminated when lapCounter > 3 if(!terminated) { float direction = GetFilteredSensorValue(); if (lapCounter > 3) { SetMotorSpeed(0); MoveServo(0); terminated = false; status = "Terminated"; updateScreen = true; driveFlag = false; lapCounter = 0; continue; } targetDirection = direction * 45.0; if (absFloat(targetDirection) >= absFloat(currentDirection)) { currentDirection = targetDirection; } else { currentDirection += (targetDirection - currentDirection); } MoveServo(currentDirection); if (driveFlag) { //Calculate new motor speed and accelerate/deccelerate float desiredPulsePerSecond = 9 + (1.0 - absFloat(direction)) * 5; float difference = desiredPulsePerSecond - tachoPulsesPerSecond; float coefficient = 0.015; if (difference < 0) { coefficient = -0.015; } currentSpeed += 20 * coefficient; if(currentSpeed>80) { currentSpeed = 80; } if(currentSpeed<0) { currentSpeed = 0; } SetMotorSpeed(currentSpeed); } } } return 0; }
int main( int argc , char * argv[] ) { ksock_t server; int rc; unsigned char buf[16]; unsigned char a,b,c,d; int clntSocket; char echoBuffer[RCVBUFSIZE]; /* Buffer for echo string */ int recvMsgSize; /* Size of received message */ int port; /* The server port to open */ pthread_t sensor_task; pthread_t sound_task; if(argc > 1) port = atoi(argv[1]); else port = 344; /* Various initialization */ /* Set the libkorebot debug level - Highly recommended for development. */ kb_set_debug_level( 2 ); /* Global libkorebot init */ if((rc = kb_init( argc , argv )) < 0 ) return 1; /* Korebot socket module init */ if((rc = ksock_init('\n')) < 0 ) return 1; /* Open the motor devices */ motor1 = knet_open( "KoreMotor:PriMotor1", KNET_BUS_ANY, 0 , NULL ); if(!motor1) { printf("Cannot open motor 0\r\n"); return 1; } motor0 = knet_open( "KoreMotor:PriMotor2", KNET_BUS_ANY, 0 , NULL ); if(!motor0) { printf("Cannot open motor 1\r\n"); return 1; } motor3 = knet_open( "KoreMotor:PriMotor3", KNET_BUS_ANY, 0 , NULL ); if(!motor3) { printf("Cannot open motor 2\r\n"); return 1; } motor2 = knet_open( "KoreMotor:PriMotor4", KNET_BUS_ANY, 0 , NULL ); if(!motor2) { printf("Cannot open motor 3\r\n"); return 1; } /* KoreMotor Comm test */ kmot_GetFWVersion( motor1, &rc); printf("Motor 1 Firmware v%u.%u\n" , KMOT_VERSION(rc) , KMOT_REVISION(rc)); /* Intialize motor controller */ InitMotor(motor0); InitMotor(motor1); InitMotor(motor2); InitMotor(motor3); kmot_SearchLimits(motor0, 10, 3, &minpos0, &maxpos0,100000); printf("motor0: min:%ld max:%ld\n\r",minpos0, maxpos0); kmot_SearchLimits(motor1, 10, 3, &minpos1, &maxpos1,100000); printf("motor1: min:%ld max:%ld\n\r",minpos1, maxpos1); kmot_SearchLimits(motor2, 10, 3, &minpos2, &maxpos2,100000); printf("motor2: min:%ld max:%ld\n\r",minpos2, maxpos2); kmot_SearchLimits(motor3, 10, 3, &minpos3, &maxpos3,100000); printf("motor3: min:%ld max:%ld\n\r",minpos3, maxpos3); kmot_SetBlockedTime(motor0,5); kmot_SetBlockedTime(motor2,5); /* Network setup - Create a ksock server */ ksock_server_open(&server, port); ksock_add_command("movecamera",2,2,koa_net_camera); ksock_add_command("movecamera2",2,2,koa_net_camera2); ksock_add_command("moveboth",2,2,koa_net_bothcam); ksock_add_command("initcamera",0,0,koa_net_init); list_command(); /* Network connection handling */ clntSocket = ksock_next_connection(&server); for(;;) { rc = ksock_get_command(clntSocket, echoBuffer, RCVBUFSIZE); if(rc>0) { ksock_exec_command(echoBuffer); /* Echo message back to client */ if (send(clntSocket, echoBuffer, rc, 0) != rc) DieWithError("send() failed"); } else switch(rc) { case 0 : break; case -3 : printf("Client disconnected\r\n"); clntSocket = ksock_next_connection(&server); break; default : printf("Socket error: %d\r\n",rc); clntSocket = ksock_next_connection(&server); break; } } close(clntSocket); /* Close client socket */ return 0; }
int main( int argc , char * argv[] ) { unsigned int ver; int rc; char * name; int32_t position,speed,current; knet_dev_t *motor0, *motor1; int32_t minpos0,maxpos0,minpos1,maxpos1; int32_t tgtmin0,tgtmax0,tgtmax1,tgtmin1; unsigned char status0,erreur0,status1,erreur1; int32_t pos0,pos1; unsigned counter = 10, addr0, addr1; /*! \todo check args should be rewriten using libpopt */ if(argc < 3) { //printf("Usage: pantilt [-a] motor0 motor1 [nb cycle]\r\n"); printf("Usage: pantilt motor0 motor1 [nb cycle]\r\n"); //printf("\t-a to use alternate address (unsuported)\r\n"); printf("\tmotor number is 1,2,3 or 4\r\n"); printf("\tnb cycle is 10 by default\r\n"); return 0; } if(argc > 3) counter = atoi(argv[3]); else counter = 10; /* Set the libkorebot debug level - Highly recommended for development. */ kb_set_debug_level(2); if((rc = kb_init( argc , argv )) < 0 ) return 1; printf("K-Team Pantilt Test Program\r\n"); /* Open the motor devices */ motor0 = PantiltOpen(atoi(argv[1])); if(!motor0) { printf("Cannot open motor %d\r\n",atoi(argv[1])); return 1; } motor1 = PantiltOpen(atoi(argv[2])); if(!motor1) { printf("Cannot open motor %d\r\n",atoi(argv[2])); return 1; } /* read controller software version */ kmot_GetFWVersion( motor1, &ver ); printf("Motor 1 Firmware v%u.%u\n" , KMOT_VERSION(ver) , KMOT_REVISION(ver)); /* Intialize motor controller */ InitMotor(motor0); InitMotor(motor1); kmot_SearchLimits(motor0, 5, 3, &minpos0, &maxpos0,100000); tgtmin0 = minpos0 + TGT_MARGIN; tgtmax0 = maxpos0 - TGT_MARGIN; printf("motor0: min:%ld max:%ld\n\r",minpos0, maxpos0); kmot_SearchLimits(motor1, 5, 3, &minpos1, &maxpos1,100000); tgtmin1 = minpos1 + TGT_MARGIN; tgtmax1 = maxpos1 - TGT_MARGIN; printf("motor1: min:%ld max:%ld\n\r",minpos1, maxpos1); printf("%d: set pos: %ld,%ld\n\r",counter,tgtmin0,tgtmin1); #if 0 kmot_SetPoint(motor0, kMotRegPosProfile, tgtmin0); kmot_SetPoint(motor1, kMotRegPosProfile, tgtmin1); #else kmot_SetPoint(motor0, kMotRegPos, tgtmin0); kmot_SetPoint(motor1, kMotRegPos, tgtmin1); #endif while(counter) { usleep(100000); printf("%d: set pos: %ld,%ld\n\r",counter,tgtmin0,tgtmin1); #if 0 kmot_SetPoint(motor0, kMotRegPosProfile, tgtmin0); kmot_SetPoint(motor1, kMotRegPosProfile, tgtmin1); #else kmot_SetPoint(motor0, kMotRegPos, tgtmin0); kmot_SetPoint(motor1, kMotRegPos, tgtmin1); #endif #if 0 kmot_GetStatus(motor0,&status0,&erreur0); kmot_GetStatus(motor1,&status1,&erreur1); while( ! ((status0 & 0x8) && (status1 & 0x8))) { kmot_GetStatus(motor0,&status0,&erreur0); kmot_GetStatus(motor1,&status1,&erreur1); } #else pos0 = kmot_GetMeasure(motor0,kMotMesPos); pos1 = kmot_GetMeasure(motor1,kMotMesPos); while( abs(pos0 - tgtmin0) > MARGIN || abs(pos1 - tgtmin1) > MARGIN) { pos0 = kmot_GetMeasure(motor0,kMotMesPos); pos1 = kmot_GetMeasure(motor1,kMotMesPos); } #endif kmot_SetMode(motor0,2); kmot_SetMode(motor1,2); usleep(100000); printf("%d: set pos: %ld,%ld\n\r",counter, tgtmax0,tgtmax1); #if 0 kmot_SetPoint(motor0, kMotRegPosProfile, tgtmax0); kmot_SetPoint(motor1, kMotRegPosProfile, tgtmax1); #else kmot_SetPoint(motor0, kMotRegPos, tgtmax0); kmot_SetPoint(motor1, kMotRegPos, tgtmax1); #endif #if 0 kmot_GetStatus(motor0,&status0,&erreur0); kmot_GetStatus(motor1,&status1,&erreur1); while( ! ((status0 & 0x8) && (status1 & 0x8))) { kmot_GetStatus(motor0,&status0,&erreur0); kmot_GetStatus(motor1,&status1,&erreur1); } #else pos0 = kmot_GetMeasure(motor0,kMotMesPos); pos1 = kmot_GetMeasure(motor1,kMotMesPos); while( abs(pos0 - tgtmax0) > MARGIN || abs(pos1 - tgtmax1) > MARGIN) { pos0 = kmot_GetMeasure(motor0,kMotMesPos); pos1 = kmot_GetMeasure(motor1,kMotMesPos); } #endif kmot_SetMode(motor0,2); kmot_SetMode(motor1,2); counter--; } }
void Init() { InitMotor(); }
int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration----------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* Configure the system clock */ SystemClock_Config(); /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_SPI2_Init(); MX_TIM1_Init(); MX_TIM2_Init(); /* USER CODE BEGIN 2 */ InitMotor(); HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL); /* USER CODE END 2 */ /* USER CODE BEGIN RTOS_MUTEX */ /* add mutexes, ... */ /* USER CODE END RTOS_MUTEX */ /* Create the semaphores(s) */ /* definition and creation of stopMoveByTime */ osSemaphoreDef(stopMoveByTime); stopMoveByTimeHandle = osSemaphoreCreate(osSemaphore(stopMoveByTime), 1); /* definition and creation of suspendMoveByTime */ osSemaphoreDef(suspendMoveByTime); suspendMoveByTimeHandle = osSemaphoreCreate(osSemaphore(suspendMoveByTime), 1); /* USER CODE BEGIN RTOS_SEMAPHORES */ xSemaphoreTake(stopMoveByTimeHandle, 0); xSemaphoreTake(suspendMoveByTimeHandle, 0); /* add semaphores, ... */ /* USER CODE END RTOS_SEMAPHORES */ /* Create the timer(s) */ /* definition and creation of elapsedTimer */ osTimerDef(elapsedTimer, elapsedTimerCallback); elapsedTimerHandle = osTimerCreate(osTimer(elapsedTimer), osTimerPeriodic, NULL); /* definition and creation of moveTimer */ osTimerDef(moveTimer, moveTimerCallback); moveTimerHandle = osTimerCreate(osTimer(moveTimer), osTimerPeriodic, NULL); /* USER CODE BEGIN RTOS_TIMERS */ /* start timers, add new ones, ... */ /* USER CODE END RTOS_TIMERS */ /* Create the thread(s) */ /* definition and creation of defaultTask */ osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 64); defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL); /* definition and creation of buttonScanTask */ osThreadDef(buttonScanTask, buttonScanFunc, osPriorityLow, 0, 128); buttonScanTaskHandle = osThreadCreate(osThread(buttonScanTask), NULL); /* definition and creation of guiTask */ osThreadDef(guiTask, guiFunc, osPriorityNormal, 0, 128); guiTaskHandle = osThreadCreate(osThread(guiTask), NULL); /* definition and creation of motorTask */ osThreadDef(motorTask, motorFunc, osPriorityAboveNormal, 0, 64); motorTaskHandle = osThreadCreate(osThread(motorTask), NULL); /* USER CODE BEGIN RTOS_THREADS */ /* add threads, ... */ /* USER CODE END RTOS_THREADS */ /* Create the queue(s) */ /* definition and creation of buttonEvents */ osMessageQDef(buttonEvents, 16, uint16_t); buttonEventsHandle = osMessageCreate(osMessageQ(buttonEvents), NULL); /* definition and creation of encoderEvents */ osMessageQDef(encoderEvents, 16, uint16_t); encoderEventsHandle = osMessageCreate(osMessageQ(encoderEvents), NULL); /* USER CODE BEGIN RTOS_QUEUES */ /* add queues, ... */ xInputEvents = xQueueCreate( 8, sizeof( struct Event ) ); xMotorEvents = xQueueCreate( 4, sizeof( struct MotorEvent ) ); /* USER CODE END RTOS_QUEUES */ /* Start scheduler */ osKernelStart(); /* We should never get here as control is now taken by the scheduler */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ }