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 */
}
