示例#1
0
/* 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 );
}
示例#2
0
/***********************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;
    
    }
}
示例#3
0
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;
}