void init_interrupts() {
uart_config uart1_config;
uart_interrupt_config uart1_int_config;
int mask;
asm("move.l %a0, -(%a7)");
asm("move.l %d0, -(%a7)");
//
// Initialize the VBR
//
asm("move.l #0x10000000, %a0");
asm("movec.l %a0, %vbr");
//
// Install the system_call function into the first vector in the VBR
//
asm("move.l #system_call, %d0");
asm("move.l %d0, 0x10000080");
//
// Setup the timer to use auto-vectored interrupt level 6, priority 3, at 1ms
//
TIMER0_ICR = 0x9B;
TIMER0_TRR = 180;
TIMER0_TMR = 0xF93B;
asm("move.l #timer_isr, %d0");
asm("move.l %d0, 0x10000078");
//
// Setup UART
//
asm("move.l #uart_isr, %d0");
asm("move.l %d0, 0x10000100");
uart1_config.vector = 64;
init_uart1(&uart1_config);
uart1_int_config.tx_rdy = false;
uart1_int_config.rx_rdy = true;
uart1_set_interrupts(&uart1_int_config);
mask = SIM_IMR;
// Timer 0 interrupt enabled
mask &= 0x0003ddff;
// Timer 0 interrupt disabled (for profiling)
//mask &= 0x0003dfff;
SIM_IMR = mask;
asm("move.l (%a7)+, %d0");
asm("move.l (%a7)+, %a0");
}
/**
* @brief: UART2 must be init before call this function
*
*/
void trace_init(void)
{
_mqx_uint err;
err = _mutex_init(&g_trace_mutex, NULL);
ASSERT_PARAM(err == MQX_EOK);
// init_uart0(uart0_irq_handler);
init_uart1(uart1_irq_handler);
}
int main()
{
TRISA = 0;
TRISB = BIT(0)|BIT(1)|BIT(3)|BIT(7)|BIT(12);
TRISC = 0;
OUTER_LED = OFF;
INNER_LED = OFF;
init_pll();
peripheral_pin_config();
init_uart1();
init_spi();
init_adc();
init_timer1();
TX_string("Wait\r\n");
delay(1000);
TX_string("Start\r\n");
T1CONbits.TON = 1;
//remove first trash reading
tiltX = accelRead(XINCL);
gRead = gyroRead(GRATE);
while(1)
{
if( newData == TRUE)
{
TX_snum5(gRead);
TX('\t');
TX_snum5(tilt);
TX('\t');
TX_snum5(x_00);
TX_string("\r\n");
OUTER_LED = OFF;
newData = FALSE;
}
}
return 0;
};
int main(void) {
// Configure clock
init_clock();
// Configure I/O PORTA
TRISAbits.TRISA0 = 0;
LATAbits.LATA0 = 0;
init_uart1();
__delay_ms(5);
while (1) {
// // Receiving by Polling
// /* Check for receive errors */
// if(U1STAbits.FERR == 1) {
// continue;
// }
// /* Must clear the overrun error to keep UART receiving */
// if(U1STAbits.OERR == 1) {
// U1STAbits.OERR = 0;
// continue;
// }
// /* Get the data */
// if(U1STAbits.URXDA == 1) {
// ReceivedChar = U1RXREG;
// }
//U1TXREG = '0'; // Transmit one character
U1TXREG = ReceivedChar; // Transmit one character
//LATAbits.LATA0 = ~LATAbits.LATA0;
__delay_ms(100);
}
while (1) { }
}
void myfunc() {
int num_rows,k, l;
double orient,delta_orient,rot_speed,t_rot,t_trans;
double x0,y0,x,y,dx,dy;
float distance,distance1,lala;
double PI=3.14159265;
x0=0;
y0=0;
double orient0;
orient0=0; // starting conditions still have to be properly intialized
double D = 0.094; /*distance between wheels, has to be measured*/
char ch;
const float tol = 1.0e-7; // 7 significant figures
float xold, xnew; // local variables
int test;
/*Read in coords, mat[d][0] contains x-coords, mat[d][1] contains y-coords*/
int i=0,j=0;
double mat[100][2];
double z;
// UInt32* pathx;
// UInt32* pathy;
// Here we should store the path-coordinates
//
// while((line=fgets(buffer,sizeof(buffer),fp))!=NULL)
// {
//
// record = strtok(line,","); /*break up string, return first token */
// while(record != NULL)
// {
// printf("record : %s \n",record); //here you can put the record into the array as per your requirement.
// mat[i][j++] = atof(record) ; /*Convert string to double, put in [i,j] then j+1*/
// record = strtok(NULL,",");
// }
// ++i ;
// j=0;
// }
initRTC();
camera_reset(160);
init_uart1(57600);
while(1){
if(getsignal()){
ch = getch();
printf("Character on serial %c 1\r\n", ch );
if(ch == 75)
{return;}
else{
switch (ch) {
case '1':
/*Read in csv-file, mat[d][0] contains distance in m, mat[d][1] contains delta_orientation in degrees */
mat[0][0]=0.61;
mat[0][1]=0;
mat[1][0]=0.58;
mat[1][1]=90;
mat[2][0]=1.00;
mat[2][1]=-45;
mat[3][0]=0.33;
mat[3][1]=-60;
num_rows = 4; // to be determined depends on how weread in givens
k=0;
while(k<=num_rows-1) {
distance=mat[k][0];
delta_orient=mat[k][1];
if(delta_orient>0){
t_rot=1000*(0.010*delta_orient+0.003);
}
else{
if(delta_orient<0){
t_rot=1000*(-0.010*delta_orient+0.003);
}
else t_rot=0;
}
t_trans=1000*(4.8*distance+0.05);
printf("tekst 2 distance: %lf,delta_orient:%lf \n t_rot: %lf t_trans: %lf \n\n",distance*1000,delta_orient,t_rot,t_trans);
if (delta_orient>0){
sendReceive(203,22,202,20);//turn counterclockwise
delay(t_rot);
sendReceive(201,0,202,0);//stop
}
if(delta_orient<0){
sendReceive(201, 22, 204, 20);//turn clockwise
delay(t_rot);
sendReceive(201,0,202,0);//stop
}
sendReceive(201, 23, 202, 20);//drive forward
delay(t_trans);
sendReceive(201, 0, 202, 0);//stop
printf("k: %d",k);
k++;
}
break;
case '5': /* Drive forward */
sendReceive(201, 23, 202, 20);
/* 201: Right motor moves forward
202: Left motor moves forward
Forward= opposite the antenna
*/
break;
case '6': /* Stop */
sendReceive(201, 0, 202, 0);
break;
case '7':
sendReceive(203, 22, 204, 20);
/* 203: Right motor moves backward
204: Left motor moves backward
Backward= antenna - side
*/
break;
case '8': /* Rotate left (counter clockwise) */
sendReceive(203, 22, 202, 20);
/* Rotate: left motor rotates backward; right one forward */
break;
case '9': /* Rotate right (clockwise) */
sendReceive(201,22,204,20);
/* Rotate: left motor rotates forward; right one backward */
break;
case 'a':
printf("Reading path");
l = readNumber();
printf("l = ",l);
followPath(l);
break;
}
}
}
} //end while
} //end myfunc
void init_uart(void)
{
init_uart0();
init_uart1();
}
