Пример #1
0
int8_t RotaryEncoder::readNav() {
    static int8_t inc = 0;
    inc += readEncoder();
    if ( inc/4 ) {
        int8_t retVal = inc/4;
        inc = 0;
        return retVal;
    }
    return 0;
}
Пример #2
0
float getPositionMine(int group){
    float val=0;
    if(group<numPidMotors){
        if(pidGroups[group].config.Enabled || vel[group].enabled)
            val = readEncoder(group);
    }else{
        val = getHeaterTempreture(group);
    }

    return val;
}
void production_control_timer(xTimerHandle pxTimer){ // Executed every millisecond
	set_debug(DEBUG8, true);
	set_led(LED3, true);

	// Get Set-Point from supervisor through IOI though FIFO
	set_debug(DEBUG7, true);
	plantParams.set_point = getSetPoint();
	plantParams.theta_des = plantParams.set_point*pi/180;

	//xil_printf("%d\n\r", plantParams.set_point); // uncomment if you want to print set-point at each control cycle to reconfig UART

	// Read the two encoder sensors
	set_debug(DEBUG7, false);
	plantParams.encoder_theta = -readEncoder(SS_ENCODER_S) % 4096;
	plantParams.thetaR = plantParams.encoder_theta*Kenc;
	set_debug(DEBUG7, true);
	plantParams.encoder_alpha= 4096+(-readEncoder(SS_ENCODER_P) % 4096);
	plantParams.alphaR = plantParams.encoder_alpha*Kenc-pi;

	// Calculate the control algorithm if pendulum is upright
	set_debug(DEBUG7, false);
	if((plantParams.alphaR >= 0 ? plantParams.alphaR:-plantParams.alphaR) < (20.*pi/180)){
		// Comment out whatever controller you don't want to use
		plantParams.u = calculateKalmanControlSignal(&plantParams);
		//plantParams.u = calculateStateFeedbackControlSignal(&plantParams);
	}
	else {
		plantParams.u = 0;
	}

	// Write voltage to the servo
	set_debug(DEBUG7, true);
	writeDAC(plantParams.u);
	set_debug(DEBUG7, false);
	++plantParams.cycle_count;

	++cycleCounter;

	set_led(LED3, false);
	set_debug(DEBUG8, false);
}
Пример #4
0
int main(void)
{
/*
	if(wiringPiSetupGpio()==-1)
	return 1;

	int gpio1 = 27;
	softPwmCreate(gpio1, 0, 50);
	usleep(100000);	
	while (1)
	{
		softPwmWrite(gpio1, 10);
		usleep(100000);	
	}

	return 0;
*/
	//if(wiringPiSetupGpio()==-1)
	//return 1;

	const int desiredDistFromWall = 110 ;

	wiringPiSetup();
	pinMode(gpio_steering,PWM_OUTPUT);
	pinMode(gpio_lidarPan,PWM_OUTPUT);
	pwmSetMode(PWM_MODE_MS);
	pwmSetClock(375);
	pwmSetRange (1024);
	
	

	softPwmCreate(gpio1, 0, 200);
	softPwmCreate(gpio2, 0, 200);

	
	//softPwmCreate(gpio_pixyPan , 0, 200);
	
	//softPwmCreate(gpio_pixyTilt, 0, 200);

	softPwmCreate(gpio_lidarTilt,0, 200);

	//initializeServos(gpio_steering,gpio_pixyPan,gpio_pixyTilt,gpio_lidarPan,gpio_lidarTilt);
	
	
	int lidFd = wiringPiI2CSetup(0x62);
	int fd = wiringPiI2CSetup(0x30);
	
	int newFd = changeEncoderAddress(fd, 0x10);

	//printf("fd = %d\r\n", newFd);
	double result = readEncoder(newFd);


	int fd2 = wiringPiI2CSetup(0x30);
	int newFd2 = changeEncoderAddress(fd2, 0x11);
	//printf("fd2 = %d\r\n", newFd2);
	double result2 = readEncoder(newFd2);
	


	double dt = 100000;
	double integral = 0;
	double previousError = 0;
	double Kp = 0.01;
	double Ki = 0.0008;
	double Kd = 1;

	double error = 0;
	double derivative = 0;
	double output = 0;
	int lidVal = 0;

	int lidarPanServoInd [3] = {20 , 50, 70};
	int pixyPanServoInd  [3] = {0 ,  5, 10}; 

	//	setServo(gpio_pixyTilt,22);
	//	usleep(30000);
	//	setServo(gpio_pixyTilt,0);

		setServo(gpio_lidarTilt,20);
		usleep(30000);
		setServo(gpio_lidarTilt,0);

		setHardServo(gpio_lidarPan,70);
		
		double avg = desiredDistFromWall;
		int errFromWall=0;

	for (int i = 0; i < 200; i++)
	{

		
		lidVal = readLidar (lidFd);
		
		if (lidVal >0 )
		{ // steering based on lidVal
		
			avg = (lidVal+avg*3)/(3.0+1) ;		

		}
		errFromWall = avg-desiredDistFromWall;
		if (errFromWall>5)
			setHardServo(gpio_steering,60);
		else if (errFromWall<-5)
			setHardServo(gpio_steering,40);
		else
			setHardServo(gpio_steering,50);


		printf ("\n lidVal: %d",lidVal) ;
		printf ("\n Avg distance: %f",avg) ;
		//setServo(gpio_steering,i);
		


		result = readEncoder(newFd);
		result2 = readEncoder(newFd2);
		error = result + result2 ; 
		integral += error ; 
		derivative = (error - previousError)/dt ; 

		output = Kp*error + Ki*integral  + Kd * derivative;
		printf ("\n LEFT ENCODER: %f -- RIGHT encoder: %f  -- Error: %f \n", result,result2,error);
		moveForward (17 , output) ;
		usleep(dt);
	}
	
	setHardServo(gpio_steering,0);
	setHardServo(gpio_lidarPan,0);
	return 0;
}
Пример #5
0
/* Run a command.  Commands are defined in commands.h */
int runCommand() {
  int i = 0;
  char *p = argv1;
  char *str;
  int pid_args[4];
  arg1 = atoi(argv1);
  arg2 = atoi(argv2);

  switch(cmd) {
  case GET_TICKS:
	Serial.println(ticks);
	break;
  case GET_BAUDRATE:
    Serial.println(BAUDRATE);
    break;
  case ANALOG_READ:
    Serial.println(analogRead(arg1));
    break;
  case DIGITAL_READ:
    Serial.println(digitalRead(arg1));
    break;
  case ANALOG_WRITE:
    analogWrite(arg1, arg2);
    Serial.println("OK");
    break;
  case DIGITAL_WRITE:
    if (arg2 == 0) digitalWrite(arg1, LOW);
    else if (arg2 == 1) digitalWrite(arg1, HIGH);
    Serial.println("OK");
    break;
  case PIN_MODE:
    if (arg2 == 0) pinMode(arg1, INPUT);
    else if (arg2 == 1) pinMode(arg1, OUTPUT);
    Serial.println("OK");
    break;
  case PING:
    Serial.println(Ping(arg1));
    break;
#ifdef USE_SERVOS
  case SERVO_WRITE:
    servos[arg1].write(arg2);
    Serial.println("OK");
    break;
  case SERVO_READ:
    Serial.println(servos[arg1].read());
    break;
#endif

#ifdef USE_BASE

  case READ_ENCODERS:
    Serial.print(readEncoder(LEFT));
    Serial.print(" ");
    Serial.println(readEncoder(RIGHT));
    break;
   case RESET_ENCODERS:
    resetEncoders();
    Serial.println("OK");
    break;
   case READ_MOTORS:
	   Serial.print(leftPID.output);
	   Serial.print(" ");
	   Serial.println(rightPID.output);
	   break;
  case MOTOR_SPEEDS:
    /* Reset the auto stop timer */
    lastMotorCommand = millis();
    if (arg1 == 0 && arg2 == 0) {
      setMotorSpeeds(0, 0);
      moving = 0;
      resetPID();
    }
    else moving = 1;
    leftPID.TargetTicksPerFrame = arg1;
    rightPID.TargetTicksPerFrame = arg2;
    Serial.println("OK");
    break;
  case UPDATE_PID:
    while ((str = strtok_r(p, ":", &p)) != '\0') {
       pid_args[i] = atoi(str);
       i++;
    }
    Kp = pid_args[0];
    Kd = pid_args[1];
    Ki = pid_args[2];
    Ko = pid_args[3];
    Serial.println("OK");
    break;
#endif
  default:
    Serial.println("Invalid Command");
    break;
  }
}
/*
void enc_poll_nonblocking_tick(const uint8_t bogus __attribute__((unused)))
  Description: Reads encoders values.

  Returns: void

  Encoders connections:
    0 -> Port I2C1
    1 -> Port SPI
    2 -> Port I2C3
*/
void enc_poll_nonblocking_tick(const uint8_t encoderNumber)
{
  static uint8_t i2cPort1Updated = 0;
  static uint8_t i2cPort3Updated = 0;
  int result;

  switch(enc_poll_state)
  {
    case EPS_DONE:
        // printf("encoder\n");
        result = setEncoderRegister(1, AS5048B_ANGLLSB_REG, SPI_TIMEOUT);
        result = readEncoder(1, SPI_TIMEOUT); // (spiPort, encoderNumber, timeout)
        
        if (isBusyI2CPort(handPorts.encoder[0]) && isBusyI2CPort(handPorts.encoder[2]))
        {
          // printf("neither 0 nor 2\n");
          enc_poll_state = EPS_I2C;
          i2cPort1Updated = 0;
          i2cPort3Updated = 0;
        }
        else if (!isBusyI2CPort(handPorts.encoder[0]) && !isBusyI2CPort(handPorts.encoder[2]))
        {
          // printf("0 and 2, EPS_DONE\n");
          result = setEncoderRegister(0, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
          result = readEncoder(0, I2C_TIMEOUT);  // (i2cPort, encoderNumber, timeout)
          result = setEncoderRegister(2, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
          result = readEncoder(2, I2C_TIMEOUT);  // (i2cPort, encoderNumber, timeout)
          enc_poll_state = EPS_DONE;
        } 
        else if (!isBusyI2CPort(handPorts.encoder[2]))
        {
          // printf("0\n");
          result = setEncoderRegister(0, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
          result = readEncoder(0, I2C_TIMEOUT);  // (i2cPort, encoderNumber, timeout)
          enc_poll_state = EPS_DONE;
        }
        else if (!isBusyI2CPort(handPorts.encoder[0]))
        {
          // printf("2\n");
          result = setEncoderRegister(2, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
          result = readEncoder(2, I2C_TIMEOUT);  // (i2cPort, encoderNumber, timeout)
          enc_poll_state = EPS_DONE;
        }      
      break;
    case EPS_I2C:
        if (!isBusyI2CPort(handPorts.encoder[0]) && !isBusyI2CPort(handPorts.encoder[2]))
        {
          // printf("0 and 2, EPS_I2C\n");
          result = setEncoderRegister(0, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
          result = readEncoder(0, I2C_TIMEOUT);  // (i2cPort, encoderNumber, timeout)
          result = setEncoderRegister(2, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
          result = readEncoder(2, I2C_TIMEOUT);  // (i2cPort, encoderNumber, timeout)
          enc_poll_state = EPS_DONE;
        } 
        else if (!isBusyI2CPort(handPorts.encoder[2]) && i2cPort1Updated == 0)
        {
          // printf("0\n");
          result = setEncoderRegister(0, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
          result = readEncoder(0, I2C_TIMEOUT);  // (i2cPort, encoderNumber, timeout)
          i2cPort1Updated = 1;
          if (i2cPort3Updated == 1)
            enc_poll_state = EPS_DONE;
        }
        else if (!isBusyI2CPort(handPorts.encoder[0]) && i2cPort3Updated == 0)
        {
          // printf("2\n");
          result = setEncoderRegister(2, AS5048B_ANGLLSB_REG, I2C_TIMEOUT);
          result = readEncoder(2, I2C_TIMEOUT);  // (i2cPort, encoderNumber, timeout)
          i2cPort3Updated = 1;
          if (i2cPort1Updated == 1)
            enc_poll_state = EPS_DONE;
        }
      break;
    default:
      enc_poll_state = EPS_DONE; // shouldn't get here
      break;

      result = result + 1 - 1;
      // printf("%d\n", result); // rmelo19, to correct, change to error checking.
  }
}
Пример #7
0
void TC5_Handler()
{

  //unedited/old:
  /*  // TcCount16* TC = (TcCount16*) TC3; // get timer struct
    if (TC5->COUNT16.INTFLAG.bit.OVF == 1) {  // A overflow caused the interrupt
        interrupted = 1;

      TC5->COUNT16.INTFLAG.bit.OVF = 1;    // writing a one clears the flag ovf flag
    //  irq_ovf_count++;                 // for debug leds
    }

    // if (TC->INTFLAG.bit.MC0 == 1) {  // A compare to cc0 caused the interrupt
    //  digitalWrite(pin_mc0_led, LOW);  // for debug leds
     // TC->INTFLAG.bit.MC0 = 1;    // writing a one clears the flag ovf flag
    // } */



  ///new
  // TcCount16* TC = (TcCount16*) TC3; // get timer struct
  if (TC5->COUNT16.INTFLAG.bit.OVF == 1) {  // A overflow caused the interrupt




    a = readEncoder();
    y = lookup_angle(a);


    if ((y - y_1) < -180.0) {
      wrap_count += 1;
    }
    else if ((y - y_1) > 180.0) {
      wrap_count -= 1;
    }
    //y_1 = y;  pushed lower

    yw = (y + (360.0 * wrap_count));






    switch (mode) {

      case 'x':
        e = (r - yw);

        ITerm += (pKi * e);
        if (ITerm > 150) ITerm = 150;
        else if (ITerm < -150) ITerm = -150;


        u = ((pKp * e) + ITerm - (pKd * (yw - yw_1))); //ARDUINO library style
        //u = u+lookup_force(a)-20;
        //   u = u_1 + cA*e + cB*e_1 + cC*e_2;     //ppt linked in octave script

        //  u = 20*e;//



        break;

      case 'v':


        e = (r - ((yw - yw_1) * 500));//416.66667)); degrees per Tc to rpm

        ITerm += (vKi * e);
        if (ITerm > 200) ITerm = 200;
        else if (ITerm < -200) ITerm = -200;


        u = ((vKp * e) + ITerm - (vKd * (yw - yw_1)));//+ lookup_force(a)-20; //ARDUINO library style

        break;

      case 't':
        u = 1.0 * r ;//+ 1.7*(lookup_force(a)-20);
        break;

      default:
        u = 0;
        break;
    }





//
//    if (u > 0) {
//      PA = 1.8;
//    }
//    else {
//      PA = -1.8;
//    }
//
//    y += PA;



    if (u > 0) {
      y+=PA;
    }
    else {
      y -=PA;
    }





    if (u > uMAX) {                          //saturation limits max current command
      u = uMAX;
    }
    else if (u < -uMAX) {
      u = -uMAX;
    }



    U = abs(u);       //+lookup_force((((a-4213)%16384)+16384)%16384)-6); ///p);//+i);


    if (abs(e) < 0.1) {
      digitalWrite(pulse, HIGH);
        SerialUSB.println(r);
    }
    else  {
      digitalWrite(pulse, LOW);
    }

    output(-y, U);  //-y

    e_3 = e_2;
    e_2 = e_1;
    e_1 = e;
    u_3 = u_2;
    u_2 = u_1;
    u_1 = u;
    yw_1 = yw;
    y_1 = y;


    TC5->COUNT16.INTFLAG.bit.OVF = 1;    // writing a one clears the flag ovf flag
  }


}