void main(void) {
InitPLL();
InitServo();
StartServo();
SetServo(90);
StartMenu();
FOREVER();
}
void StartCore(void) {
PORTB = 0xAA;
DDRB = 0xFF;
// 初始化小按键
DDRP_DDRP0 = 0;
PPSP_PPSP0 = 0;
PERP_PERP0 = 1;
/********************************/
/* 初始化 */
InitServo();
StartServo();
SetServo(90);
InitSpeeder();
irInit();
InitADC();
InitSCI0();
WaitEnable();
/*******************************/
GetBlackAndWhite();
PORTB = 0x55;
Wait(1500);
PORTB = 0xAA;
while (PTIP_PTIP0);
PORTB = 0x5A;
Wait(1000);
PORTB = 0xA5;
CoreControl();
}
void InitMovement()
{
memset(&movement, 0, sizeof(movement));
uint8_t i;
for(i = 0; i != 5; i++)
{
//ToDo: Nonsence - what is this switch here for?
switch(movement[i].type)
{
case STEPPER:
InitStepper(&(movement[i].stepper));
break;
case SERVO:
InitServo(&(movement[i].servo));
break;
case UNUSED:
break;
}
}
}
int main(void)
{
uint8_t i,variable=0,servo=0,flag_1=0,flag_2=0;
CfgClock();
CfgModUART();
InitServo();
CfgModGPIO();
//lcd_init();
//lcd_clear();
//lcd_string("Teste");
while(1)
{
// delay_ms(5);
// SendServo(servo,variable);
// if(!flag_1)
// if(variable < 180) variable++;
// else flag_1 = 1;
// else
// if(variable > 0) variable--;
// else
// {
// flag_1 = 0;
// if(servo < 5) servo++;
// else servo = 0;
// }
if(!RefreshUART)
{
RefreshUART = 1;
UARTprintf("\n[Servo(0-5),Position(0-180)]:");
}
}
}
/**
* Constructor that specifies the digital module.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The PWM channel on the digital module to which the servo is attached (1..10).
*/
Servo::Servo(uint8_t moduleNumber, uint32_t channel) : SafePWM(moduleNumber, channel)
{
InitServo();
}
/**
* Constructor that assumes the default digital module.
*
* @param channel The PWM channel on the digital module to which the servo is attached.
*/
Servo::Servo(uint32_t channel) : SafePWM(channel)
{
InitServo();
}
BOOL setMode(BYTE pin,BYTE mode){
ClearPinState(pin);
println_I("Pin :");p_int_I(pin);print_I(" is mode: ");printMode(mode,INFO_PRINT);
//BYTE pwm,dir;
if (mode == NO_CHANGE){
return TRUE;
}
switch (mode){
case HIGH_IMPEDANCE:
ClearPinState(pin);
// Return here so as not to save this state to the eeprom
return TRUE;
case IS_UART_TX:
case IS_UART_RX:
if(pin == 17 || pin == 16){
configPinMode(16,IS_UART_TX,OUTPUT,ON);
configPinMode(17,IS_UART_RX,INPUT,ON);
InitUART();
return TRUE;
}
break;
case IS_SPI_MOSI:
case IS_SPI_MISO:
case IS_SPI_SCK:
if(pin == 0 || pin == 1||pin == 2 ){
configPinMode(0,IS_SPI_SCK,INPUT,ON);
configPinMode(1,IS_SPI_MISO,INPUT,ON);
configPinMode(2,IS_SPI_MOSI,INPUT,ON);
return TRUE;
}
break;
case IS_ANALOG_IN:
configPinMode(pin,mode,INPUT,OFF);
if(InitADC(pin)){
return TRUE;
}
break;
case IS_PWM:
if(InitPWM(pin)){
return TRUE;
}
return FALSE;
case IS_DC_MOTOR_VEL:
case IS_DC_MOTOR_DIR:
if(InitDCMotor(pin)){
return TRUE;
}
return FALSE;
case IS_SERVO:
InitServo(pin);
configPinMode(pin,mode,OUTPUT,OFF);
return TRUE;
case IS_DO:
configPinMode(pin,mode,OUTPUT,OFF);
return TRUE;
case IS_DI:
case IS_PPM_IN:
case IS_COUNTER_OUTPUT_INT:
case IS_COUNTER_OUTPUT_DIR:
case IS_COUNTER_OUTPUT_HOME:
case IS_COUNTER_INPUT_INT:
case IS_COUNTER_INPUT_DIR:
case IS_COUNTER_INPUT_HOME:
configPinMode(pin,mode,INPUT,ON);
return TRUE;
default:
configPinMode(pin,mode,INPUT,ON);
return TRUE;
}
return FALSE;
}
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;
}
/**
* Constructor that specifies the digital module.
*
* @param slot The slot in the chassis that the digital module is plugged into.
* @param channel The PWM channel on the digital module to which the servo is attached.
*/
Servo::Servo(UINT32 slot, UINT32 channel) : SafePWM(slot, channel)
{
InitServo();
}
/**
* Constructor that assumes the default digital module.
*
* @param channel The PWM channel on the digital module to which the servo is attached.
*/
Servo::Servo(UINT32 channel) : SafePWM(channel)
{
InitServo();
}
/**
* @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on the MXP port
*/
Servo::Servo(uint32_t channel) : SafePWM(channel)
{
InitServo();
// printf("Done initializing servo %d\n", channel);
}
// ============================================================================
int main( void )
{
int ch = 0;
uint32_t ccount = 0;
uint32_t lastTick;
int pwm;
SystemCoreClockUpdate();
SysTick_Config( SystemCoreClock / HB_HZ);
// Enable peripheral clocks
// TODO: Remove GPIOCEN when moving to the smaller CPU
RCC->AHBENR |= (RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOCEN);
#ifdef USE_USART
InitUSART(400);
#endif // USE_USART
InitLED();
InitServo();
configButtons();
while( 1 ) {
if ( curTick > (HB_HZ) ) {
curTick -= (HB_HZ);
// Once per second processing...
}
if ( curTick != lastTick ) {
lastTick = curTick;
// On each timer tick move the turnout slightly closer to the new position
for ( int idx=0; idx<SERVO_COUNT; ++idx ) {
if ( servo[idx].currentPos < servo[idx].targetPos ) {
servo[idx].currentPos += SERVO_DELTA;
if ( servo[idx].currentPos > servo[idx].targetPos ) {
servo[idx].currentPos = servo[idx].targetPos;
}
}
else
if ( servo[idx].currentPos > servo[idx].targetPos ) {
servo[idx].currentPos -= SERVO_DELTA;
if ( servo[idx].currentPos < servo[idx].targetPos ) {
servo[idx].currentPos = servo[idx].targetPos;
}
}
}
TIM1->CCR1 = servo[SERVO1].currentPos;
TIM1->CCR2 = servo[SERVO2].currentPos;
TIM1->CCR3 = servo[SERVO3].currentPos;
TIM1->CCR4 = servo[SERVO4].currentPos;
btnCheck();
}
#ifdef USE_USART
if ( usartTxEmpty() ) {
usartWriteByte(ch+33);
++ch;
ch &= 0x3F;
}
#endif // USE_USART
}
}
