/* Extract angle from the Message */
void can_proc_move_to_angle_msg ( sCAN* mMsg )
{
// Angle is a 4 byte signed integer. It is degrees * 100
float tmp_angle = extract_float_msg( mMsg->data );
Destination.position = convert_to_value( convert_to_fixedpoint(tmp_angle) );
Destination.speed = extract_long_int( &(mMsg->data[4]) ); // [0..100%]
motor_set_duty( Destination.speed );
}
/***********************cmdparser*******************************
*
* Purpose: Parse the command string to call the correct function.
*
* Input: char *cmdtype: input command string.
*
* Output: int result: Resulting integer value.
*
***************************************************************/
void cmdparser(char *buffer) {
char cmdtype[CMD_LEN+1] = {0};
int numchars = 0;
static int numcmd = 0; // Count of number of commands parsed
static byte tog = 0; // Laser toggle bit
char motor1_pct, motor2_pct;
cmdtype[0] = buffer[numchars];
cmdtype[1] = buffer[numchars+1];
cmdtype[2] = buffer[numchars+2];
cmdtype[CMD_LEN] = '\0'; // Terminate input command after three bytes, leaving just the command type
switch(cmdconv(cmdtype)) {
case 0: // If no command found, go to next character.
seekcmd(buffer, &numchars);
break;
case PNG: // ping
SCIprintf("png%05d",numcmd); // echo command confirmation with stamp.
//LCDclear(); LCDputs("Ping!");
numcmd++;
numchars += SCI_CMDSIZ;
break;
case ABT: // STOP THE PRESS!
SCIprintf("abt%05d",numcmd);
LCDclear(); LCDputs("Abort!\nAbort!");
stop_motion();
numcmd++;
numchars += SCI_CMDSIZ;
break;
case RES: // Resume operation
SCIprintf("res%05d",numcmd);
LCDclear(); LCDputs("Resuming...");
start_motion();
LCDclear(); LCDputs("Resumed");
numcmd++;
numchars += SCI_CMDSIZ;
break;
case MOV: // Set motor speed (0% - 100%)
SCIprintf("mov%05d", numcmd);
if(buffer[numchars+3] == '2') {
// Both motors selected
TC_INT_DISABLE(TC_MOTOR); // Disable motor control law
motor_set_speed(MOTOR1C, (char)atoi(&buffer[numchars+4]));
motor_set_speed(MOTOR2C, (char)atoi(&buffer[numchars+4]));
TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law
//LCDclear(); LCDprintf("\rM%c: %3d M%c: %3d", MOTOR1C, atoi(&buffer[numchars+4]), MOTOR2C, atoi(&buffer[numchars+4]));
}
else {
motor_set_speed(buffer[numchars+3], (char)atoi(&buffer[numchars+4]));
//LCDclear(); LCDprintf("\rMotor %c: %3d", buffer[numchars+3], atoi(&buffer[numchars+4]));
}
numcmd++;
numchars += SCI_CMDSIZ;
break;
case DST: // Set motor distance (+speed)
SCIprintf("dst%05d", numcmd);
switch(buffer[numchars+4]) {
case '0': // Setting a speed
motor1_pct = motor_convert(MOTOR1C, (int)atoi(&buffer[numchars+5]));
motor2_pct = motor_convert(MOTOR2C, (int)atoi(&buffer[numchars+5]));
// Set speed to both motors if 4th char is a '2'
if(buffer[numchars+3] == '2') {
TC_INT_DISABLE(TC_MOTOR); // Disable motor control law
motor_set_speed(MOTOR1C, motor1_pct);
motor_set_speed(MOTOR2C, motor2_pct);
TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law
} else
motor_set_speed(buffer[numchars+3],
motor_convert(buffer[numchars+3], (int)atoi(&buffer[numchars+5]))
);
//LCDclear(); LCDprintf("\rM1: %3d M2: %3d", motor1_pct, motor2_pct);
//LCDprintf("\nS1: %3d S2: %3d", atoi(&buffer[numchars+5]), atoi(&buffer[numchars+5]));
break;
case '1': // Setting a distance
// Set speed to both motors if 4th char is a '2'
if(buffer[numchars+3] == '2') {
motor_set_distance(MOTOR1C, (word)atoi(&buffer[numchars+5]));
motor_set_distance(MOTOR2C, (word)atoi(&buffer[numchars+5]));
//LCDclear(); LCDprintf("\nD%c: %3d D%c: %3d", MOTOR1C, atoi(&buffer[numchars+5]), MOTOR2C, atoi(&buffer[numchars+5]));
}
else {
motor_set_distance(buffer[numchars+3], (word)atoi(&buffer[numchars+5]));
//LCDclear(); LCDprintf("\rDist %c: %3d", buffer[numchars+3], atoi(&buffer[numchars+5]));
}
break;
}
numcmd++;
numchars += SCI_CMDSIZ;
break;
case SPN: // Spin in place
SCIprintf("spn%05d", numcmd);
DisableInterrupts;
motor_set_speed(MOTOR1C, -50);
motor_set_speed(MOTOR2C, 50);
motor_set_distance(MOTOR1C, (word)atoi(&buffer[numchars+3]));
motor_set_distance(MOTOR2C, (word)atoi(&buffer[numchars+3]));
EnableInterrupts;
SCIprintf("Dist: %3d\n", atoi(&buffer[numchars+3]));
numcmd++;
numchars += SCI_CMDSIZ;
break;
case AIM: // Toggle laser pointer
SCIprintf("aim%05d",numcmd);
tog = (tog) ? 0 : 1;
PTP_PTP0 = (tog) ? 1 : 0;
numcmd++;
numchars += SCI_CMDSIZ;
break;
case STP: // Force stop; set motor PWM to zero to stop the high frequency ringing!
SCIprintf("stp%05d",numcmd);
TC_INT_DISABLE(TC_MOTOR); // Disable motor control law
motor_set_duty(MOTOR1C, 0);
motor_set_duty(MOTOR2C, 0);
TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law
numcmd++;
numchars += SCI_CMDSIZ;
break;
}
}
int main (void)
{
unsigned char value;
//Configure all ports as inputs
TRISA = 0xFFFF; TRISB = 0xFFFF;// TRISC = 0xFFFF;
//---------------------------------------------------------------------
// OSCILATOR
//---------------------------------------------------------------------
oscilator_init();
//config_load();
//---------------------------------------------------------------------
// UART
//---------------------------------------------------------------------
uart_init();
printf("Uart Init Ok\n");
//---------------------------------------------------------------------
// STATUS (LED/SOUND)
//---------------------------------------------------------------------
status_init();
// STATUS(BLINK_SLOW,_ON,_ON);
// __delay_ms(10);
// STATUS_INIT;
//---------------------------------------------------------------------
// ADC
//---------------------------------------------------------------------
// adc_init();
// timer_init_ms();
//---------------------------------------------------------------------
// I2C
//---------------------------------------------------------------------
i2c_init();
printf("i2c Init Ok\n");
ITG3200_begin();
printf("ITG3200 Ok\n");
ADXL345_begin();
printf("ADXL Init Ok\n");
//STATUS(BLINK_FAST,_ON,_ON);
printf("Init Gyro\n");
// STATUS_NORMAL;
//__delay_ms(100);
// Acceleromter calibration
int AcclCalibration;
AcclCalibration =1;
//#define AccCali
#ifdef AccCali
while(AcclCalibration){
int i;
double adjAccl;
for(i=0;i<10;i++){
ADXL345_update();
}
float Acclx=getAcclXOutput();
float Accly=getAcclYOutput();
float Acclz=getAcclZOutput();
float Gravity=sqrt(Acclx*Acclx+Accly*Accly+Acclz*Acclz);
adjAccl= 1/Gravity;
ADXL345_Scaler =adjAccl * ADXL345_Scaler;
printf("%1.3f %1.3f %1.3f Gravity %1.9f y %1.9f %1.3f\n",Acclx,Accly,Acclz,Gravity,ADXL345_Scaler,adjAccl);
if (Gravity == 1.0000) AcclCalibration=0;
}
printf("Accelerometer Calibrated \n");
#endif
//#define GyroTest
#ifdef GyroTest
while(1){
ITG3200_update();
float GyroX=getGyroXOutput();
float GyroY=getGyroYOutput();
float GyroZ=getGyroZOutput();
int ID=getGyroIDOutput();
float Temp= getGyroTempOutput();
printf("%1.3f %1.3f %1.3f %1.3f %i \n",GyroX,GyroY,GyroZ,Temp,ID);
}
#endif
//---------------------------------------------------------------------
// MOTOR
//---------------------------------------------------------------------
motor_init();
printf("motor init ok\n");
__delay_ms(100);
//#define motor_debug
#ifdef motor_debug
//while(1){
// _LAT(pinMotor0) = 1;
// _LAT(pinMotor1) = 1;
// _LAT(pinMotor2) = 1;
// _LAT(pinMotor3) = 1;
//
// _TRIS(pinMotor0) = 0;
// _TRIS(pinMotor1) = 0;
// _TRIS(pinMotor2) = 0;
// _TRIS(pinMotor3)= 0;
//
//
//}
int i=0;
while(i<50){
_LAT(pinLed1) =! _LAT(pinLed1);
motor_set_duty(0,i);
motor_set_duty(1,i);
motor_set_duty(2,i);
motor_set_duty(3,i);
motor_apply_duty();
__delay_ms(50);
i = i + 1;
// if(i> 90) i = 0;
printf("Motor %x \n");
}
__delay_ms(250);
i=0;
motor_set_duty(0,i);
motor_apply_duty();
motor_set_duty(1,i);
__delay_ms(250);
motor_apply_duty();
motor_set_duty(2,i);
__delay_ms(250);
motor_apply_duty();
motor_set_duty(3,i);
__delay_ms(250);
motor_apply_duty();
__delay_ms(250);
#endif
//---------------------------------------------------------------------
// IMU
//---------------------------------------------------------------------
imu_init();
//#define imu_debug
#ifdef imu_debug
//IMU debug comment out for production
float interval_ms=0;
while (1){
ITG3200_update();
ADXL345_update();
//interval_ms = timer_dt();
// if(interval_ms > 0 ){
//we have fresh adc samples
// printf("Time %lu ",interval_us);
imu_update(interval_ms);
float* K = dcmEst[2]; //K(body) vector from DCM matrix
float pitch_roll = acos(K[2]); //total pitch and roll, angle necessary to bring K to [0,0,1]
//cos(K,K0) = [Kx,Ky,Kz].[0,0,1] = Kz
float Kxy = sqrt(K[0]*K[0] + K[1]*K[1]);
float anglePitch = - (pitch_roll * asin(K[0]/Kxy))*50;//*180/PI ;
float angleRoll = (pitch_roll * asin(K[1]/Kxy))*50;//*180/PI;
//float angleRoll = (atan2(dcmEst[2][2],dcmEst[2][1]))*180/3.14;
//float anglePitch = -(asin(dcmEst[2][0]))*180/3.14;
// float angleYaw=(-atan2(dcmGyro[1][0],dcmGyro[0][0]))*180/3.14;
float driftX=(sin(anglePitch*(3.14/180))*(sin(angleRoll*(3.14/180))));
float CalcDriftX=getAcclXOutput()-driftX;
float Acclx=getAcclXOutput();
float Gyrox=getGyroXOutput();
float Accly=getAcclYOutput();
float Acclz=getAcclZOutput();
float Gyroy=getGyroYOutput();
//printf("\n");
//if(0 == imu_sequence % 4){
// hdlc_send_byte(float_to_int(anglePitch));
// hdlc_send_byte(float_to_int(angleRoll));
// hdlc_send_byte(float_to_int(Acclx*100));
// hdlc_send_byte(float_to_int(Gyrox*1000));
// hdlc_send_byte(float_to_int(Accly*100));
// hdlc_send_byte(float_to_int(Gyroy*1000));
// //hdlc_send_byte(float_to_int(Acclz*100));
// //hdlc_send_byte(float_to_int(Kxy*100));
// //hdlc_send_byte(float_to_int(K[0]*100));
// //hdlc_send_byte(float_to_int(K[1]*100));
// //hdlc_send_byte(float_to_int(K[2]*100));
//
//// float GyroYpitch = GyroYpitch+ (getGyroYOutput()*(interval_us/1000));
//// printf("pitch %1.3f roll %1.3f Gyro ",pitch.value,roll.value,GyroYpitch);
//// printf(" Angle P%1.3f R%1.3f Drift=%1.3f Acclx=%1.3f Calc=%1.3f GyroX=%1.3f\n",anglePitch,angleRoll,driftX,Acclx,CalcDriftX,GyroX);
// hdlc_send_sep();
//}
}
}
int main(){
uint8_t start_r, old_IPL;
uint8_t hz50_scaler, hz5_scaler, hz1_scaler, sec;
uint32_t tick = 0;
hz50_scaler = hz5_scaler = hz1_scaler = sec = 0;
touch_init();
__delay_ms(200);
lcd_initialize(); // Initialize the LCD
motor_init();
lcd_clear();
lcd_locate(0,0);
lcd_printf("-- Ball position: --");
timers_initialize(); // Initialize timers
while (1) {
start_r = 0;
while(!start_r) {
// disable all maskable interrupts
SET_AND_SAVE_CPU_IPL(old_IPL, 7);
start_r = start;
// enable all maskable interrupts
RESTORE_CPU_IPL(old_IPL);
}
// Periodic real-time task code starts here = CENTER_X + RADIUS * cos(tick * SPEED);
// Ypos_set = CENT!!!
double pidX, pidY;
uint16_t duty_us_x, duty_us_y;
// 50Hz control task
if(hz50_scaler == 0) {
calcQEI(Xpos_set, Xpos, Ypos_set, Ypos);
// Xpos_set = CENTER_X;
// Xpos_set = CENTER_Y;
Xpos_set = CENTER_X + RADIUS * cos(tick * SPEED);
Ypos_set = CENTER_Y + RADIUS * sin(tick * SPEED);
tick++;
pidX = pidX_controller(Xpos);
pidY = pidY_controller(Ypos);
// TODO: Convert PID to motor duty cycle (900-2100 us)
// setMotorDuty is a wrapper function that calls your motor_set_duty
// implementation in flexmotor.c. The 2nd parameter expects a value
// between 900-2100 us
// duty_us_x = cap((pidX*1000.0), 2100, 900);
// duty_us_y = cap((pidY*1000.0), 2100, 900);
duty_us_x = cap((pidX + 1500), 2100, 900);
duty_us_y = cap((pidY + 1500), 2100, 900);
motor_set_duty(1, duty_us_x);
motor_set_duty(2, duty_us_y+100);
// setMotorDuty(MOTOR_X_CHAN, duty_us_x);
// setMotorDuty(MOTOR_Y_CHAN, duty_us_y);
}
// 5Hz display task
if(hz5_scaler == 0) {
// lcd_locate(0,1);
// lcd_printf("Xp=%.1f,Yp=%.1f", Xpos, Ypos);
// lcd_locate(0,2);
// lcd_printf("X*=%.1f, Y*=%.1f", Xpos_set, Ypos_set);
// lcd_locate(0,3);
// lcd_printf("pX=%.1f,pY=%.1f", pidX, pidY);
// lcd_locate(0,4);
// lcd_printf("dx=%u, dY=%u", duty_us_x, duty_us_y);
//
if(deadline_miss >= 1) {
lcd_locate(0,6);
lcd_printf("%4d d_misses!!!", deadline_miss);
}
}
// 1Hz seconds display task
if(hz1_scaler == 0) {
lcd_locate(0,7);
lcd_printf("QEI: %5u", getQEI());
sec++;
}
hz50_scaler = (hz50_scaler + 1) % 2;
hz5_scaler = (hz5_scaler + 1) % 20;
hz1_scaler = (hz1_scaler + 1) % 100;
start = 0;
}
return 0;
}
