void InicializaHardware(void)
{
S12ADC_init();
S12ADC_start();
lcd_initialize();
lcd_clear();
}
/*******************************************************************************
* Function name: timer_adc
* Description : The function configures a ADC unit and a timer unit. After each
* timer period the callback function cmt_callback_readADC, is
* executed.
* Arguments : none
* Return value : none
*******************************************************************************/
void timer_adc(void)
{
/* Prepare the ADC unit for continuous conversion of pot VR1 value. */
S12ADC_init();
/* Start continuous ADC conversion. */
S12ADC_start();
/* Prepare a CMT channel as a periodic timer. */
/* Initialize a CMT one-shot timer. */
cmt_init();
/* Set up the callback function on cmt channel 0 */
cmt_callback_set(CHANNEL_1, &cmt_callback_readADC);
/* Start 100mS count on cmt channel 0. */
cmt_start(CHANNEL_1, g_timer_count);
} /* End of function timer_adc(). */
void main(void)
{
//uint16_t adc_count;
float adc_count;
/* Initialize LCD */
lcd_initialize();
S12ADC_init();
//UART initialization
sci_uart_init();
sci_tx_int_enable();
sci_rx_int_enable();
/* Clear LCD */
lcd_clear();
/* Display message on LCD */
//Message will be on Position 1 and it will be stopped
lcd_display(LCD_LINE1, " Mitch and ");
lcd_display(LCD_LINE2, " Paul ");
lcd_display(LCD_LINE3, "Train Track ");
lcd_display(LCD_LINE4, " STOPPED ");
lcd_display(LCD_LINE5, " _oo_");
lcd_display(LCD_LINE6, " Pos1 |[][]|");
lcd_display(LCD_LINE7, " |O O|");
lcd_display(LCD_LINE8, " |____|");
/*
STDOUT is routed through the virtual console window tunneled through the JTAG debugger.
Open the console window in HEW to see the output
*/
printf("This is the debug console\r\n");
/* The three pushbuttons on the YRDK board are tied to interrupt lines, set them up here */
R_SWITCHES_Init();
//Intitalize all global variables
count=0;
Forward=0;
Backword=0;
Enable=0;
//Character to input from putty
char new_char;
//Initialize Local variables
int toggle=0;
//Initialize all LED's starting at position 1
LED4=LED_ON;
LED5=LED_ON;
LED6=LED_ON;
LED7=LED_ON;
LED8=LED_OFF;
LED9=LED_OFF;
LED10=LED_OFF;
LED11=LED_OFF;
LED12=LED_OFF;
LED13=LED_OFF;
LED14=LED_OFF;
LED15=LED_OFF;
while (1)
{
S12ADC_start();
while(false==S12ADC_conversion_complete())
{}
new_char= sci_get_char();
//Through the serial communication
if(new_char =='F' ||new_char=='f') //If inputted 'F' or 'f' move Forward
{
lcd_display(LCD_LINE4, " FORWARD ");
Forward=1;
Enable=1;
Backword=0;
sci_put_string("Remote: Forward\n\r");
}
if(new_char =='S' ||new_char=='s') //If inputted 'S' or 's' stop movement
{
lcd_display(LCD_LINE4, " STOPPED ");
//Forward=0;
Enable=0;
//Backward=0;
sci_put_string("Remote: Stop\n\r");
}
if(new_char =='R' ||new_char=='r') //If inputted 'R' or 'r' move in reverse
{
lcd_display(LCD_LINE4, " REVERSE ");
Forward=0;
Enable=1;
Backword=1;
sci_put_string("Remote: Reverse\n\r");
}
adc_count= S12ADC_read();
adc_count=adc_count/4095;
int adccount=adc_count*100;
char result[20];
sprintf(result, "sp%3d%|____|", adccount);
lcd_display(LCD_LINE8, (const uint8_t *) result);
// lcd_display(LCD_LINE8, adc_count);
if(new_char=='D'||new_char=='d') //If inputted 'D' or 'd' diplay the current speed
{
char speed[20];
sprintf(speed,"Current Speed: %3d \n\r",adccount);
sci_put_string(speed);
}
if(adc_count<=.01 & Enable==1)
{
lcd_display(LCD_LINE4, " STOPPED ");
//Forward=0;
Enable=0;
//Backward=0;
sci_put_string("Remote: Stop\n\r");
while(adc_count<=0.05)
{
inloop=1;
S12ADC_start();
while(false==S12ADC_conversion_complete())
{}
adc_count= S12ADC_read();
adc_count=adc_count/4095;
}
if(LastDir==1)
{
lcd_display(LCD_LINE4, " FORWARD ");
Forward=1;
Enable=1;
Backword=0;
sci_put_string("Forward\n\r");
}
else if(LastDir==-1)
{
lcd_display(LCD_LINE4, " REVERSE ");
Forward=0;
Enable=1;
Backword=1;
sci_put_string("Reverse\n\r");
}
inloop=0;
}
else if(adc_count<=.25)
{
delayMS(1000);
}
else if(adc_count<=.5)
{
delayMS(500);
}
else if(adc_count<=.75)
{
delayMS(100);
}
else if(adc_count<=1.0)
{
delayMS(10);
}
if(Enable==1)
{
trainMove();
if(toggle==1 && Backword==1)
{
lcd_display(LCD_LINE5, " _**_");
lcd_display(LCD_LINE7, " |* *|");
toggle=0;
}
else if(toggle==0 && Backword==1)
{
lcd_display(LCD_LINE5, " _oo_");
lcd_display(LCD_LINE7, " |O O|");
toggle=1;
}
if(Forward==1)
{
lcd_display(LCD_LINE5, " _oo_");
lcd_display(LCD_LINE7, " |* *|");
}
}
else
{
lcd_display(LCD_LINE5, " _oo_");
lcd_display(LCD_LINE7, " |O O|");
}
if(Forward==1 && Enable==1) {count++; if(count==12){count=0;}}
if(Backword==1 && Enable==1){count--; if(count==-1){count=11;}}
}
}
void Setup() {
/* Initialize LCD */
lcd_initialize();
/* Clear LCD */
lcd_clear();
/* Display message on LCD */
lcd_buffer_print(LCD_LINE2, " TEST ");
/* Initialize motors */
Motors_Init();
/* Turn on motors relay */
Motors_On();
/* Send arm signal to motors */
Motor_Arm(MOTOR_UPPER);
Motor_Arm(MOTOR_BOTTOM);
/* Initialize servos */
Servos_Init();
/* Initialize sonar */
sonarInitialize(); //must be initialized before IIC, otherwise it will not work
/* Setup the 12-bit A/D converter */
S12ADC_init();
/* Initialize I2C with control */
riic_ret_t iic_ret = RIIC_OK;
iic_ret |= riic_master_init();
while (RIIC_OK != iic_ret) {
nop(); /* Failure to initialize here means demo can not proceed. */
}
/* Setup Compare Match Timer */
CMT_init();
/* Initialize PID structure used for PID properties */
PID_Init(&z_axis_PID, 0.7, 0.05, 0.30, dt, 0, 0.5); //0.7 0.05 0.15
PID_Init(&Pitch_PID, 1, 0, 0.01, dt, -30, 30);
PID_Init(&Roll_PID, 1, 0, 0.01, dt, -30, 30);
Init_AVG(0, &pitchAVG);
Init_AVG(0, &rollAVG);
/* Make the port connected to SW1 an input */
PORT4.PDR.BIT.B0 = 0;
/*MPU6050 Initialization*/
MPU6050_Test_I2C();
Setup_MPU6050();
Calibrate_Gyros();
// Calibrate_Accel();
/*Kalman Initialization*/
init_Kalman();
//MS5611-01BA01 init
// MS5611_Init();
desiredState.key.motor_diff_us = 0;
desiredState.key.abs.pos.z = 0.20;
altitudeValue = 200;
mainWDT = WDT_Init(500, Fallback);
WDT_Start(&mainWDT);
sonarWDT = WDT_Init(60, Sonar_Fallback);
WDT_Start(&sonarWDT);
}
