View unanswered posts | View active topics It is currently Mon May 25, 2015 11:56 am






Reply to topic  [ 32 posts ]  Go to page 1, 2, 3  Next
Question for MHTS 
Author Message
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Question for MHTS
I've noticed that in some of your code for teleop when looking on the forums that you bind motor vaules from -100 to 100. What is the point of this? The Motors can't go over 100 power anyway?

_________________
Thanks!


Fri Feb 06, 2015 10:42 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Question for MHTS
akash329dsouza wrote:
I've noticed that in some of your code for teleop when looking on the forums that you bind motor vaules from -100 to 100. What is the point of this? The Motors can't go over 100 power anyway?

You are correct that even if you assign motor power beyond the 100 range, RobotC will cap it. However, in our library, we have a lot of logic that will calculate PID and other feedback using the current motor power. So if they are not kept track of correctly, it could affect the calculation. Besides, our library tries to not make this kind of assumptions. What if RobotC decided to not cap the motor value in the next release, our library will still work in that case.


Sat Feb 07, 2015 3:58 am
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Question for MHTS
Another thing, we have been experimenting with the IR PID control but we notice it is always veering slightly left. Once it gets closer to the beacon it corrects it's self but it time out of time so it skews a bit left. Is there any way to fix this?

_________________
Thanks!


Fri Feb 20, 2015 12:20 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Question for MHTS
Would you show me your code? And what is your arrangement of the IR seeker(s)?


Fri Feb 20, 2015 8:42 am
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Question for MHTS
This is our code:
Code:
task main()
{
   int Speed = -30;
   int dist = USreadDist(LEGOUS123);
    while (dist < 83)
    {
       dist = USreadDist(LEGOUS123);
        float irDir = IRGetACDir(leftIR,1) + IRGetACDir(rightIR,2);
        float error = irDir - 10.0;
        nxtDisplayTextLine(3, "Error=%d,Dir=%5.1f", error, 1);
        motor[motorF] =Speed + (Kp*error);
        motor[motorG] = Speed + (Kp*error;
        motor[motorD] = Speed - (Kp*error;
        motor[motorE] = Speed - (Kp*error;
        wait1Msec(20);
    }
}


Our arrangement are that the IR Seekers are tilted slightly inward

_________________
Thanks!


Sun Feb 22, 2015 6:58 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Question for MHTS
akash329dsouza wrote:
Another thing, we have been experimenting with the IR PID control but we notice it is always veering slightly left. Once it gets closer to the beacon it corrects it's self but it time out of time so it skews a bit left. Is there any way to fix this?

Sorry, I have been busy with the aftermath of FRC bag and tag. In any case, that's a side effect of some IR seeker zones are wider angle than the others. For example, if you are far away, the two seekers will read 2 and 8 which are on the narrow zone. So it will approach the beacon very accurately. However, when you are close enough, the zones changed to 3 and 7 which are the wide zones. The assumption is that if you maintained course when you were on zones 2 and 8, you should still be right onto the IR beacon. But if for some reason the robot turned a little bit while in zones 3 and 7, because of the wider zones, it wouldn't know the misalignment until it exited zone 3 or 7, then it will correct itself and eventually entering zone 4 and 6. But at this point, you are already approaching the IR beacon at an angle. It still accurately reached the beacon but at an angle. Therefore, it is important to be tolerant to this angle. If your robot's design is sensitive to this misalignment, there are a few things you could do. First, you can adjust the two IR seekers so that the left one has zone 2 pointing right ahead but the right IR seeker should have zone 7 pointing ahead instead of zone 8 (so the sum of the ahead direction is 9 instead of 10). This will make the approach readings something like this: 2 and 7, 3 and 6, 4 and 5. What this does is to interleave a narrow zone with a wide zone as the robot approaches the target. It will sacrifice a little bit of the accuracy of two narrow zones and trade it with a more uniform approach. Another way to do it is to plan your route differently so that it will turn to the beacon when it's far away while it's on the narrow zones and then use the gyro to maintain course so when the IR seekers are on wide zones, it wouldn't accidentally turned without knowing because the gyro is keeping it straight.


Mon Feb 23, 2015 9:48 pm
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Question for MHTS
Ok thanks for the tips, just a side note, did your team make it to Super Regionals?

_________________
Thanks!


Fri Feb 27, 2015 8:37 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Question for MHTS
No ironically, our autonomous was malfunctioning during the entire competition, the scissor jack was not raising to the correct height. The problem comes and goes. When we tried to debug in the pit, it behaved correctly. When it's in competition, it misbehaved. We still managed to rank number 1 seed but lost in the final rounds. After the competition, we tried to investigate the cause but the problem never reproduced again. The code didn't really change between competitions, so it is a mystery why the problem appeared. On the bright side, I can now concentrate on FRC.
What about your team? Did you guys advance to super regional?


Fri Feb 27, 2015 8:48 pm
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Question for MHTS
Yes! We made it to super regionals in Iowa. We finished 9th place in robot runs. (We won 2nd place inspire!)

_________________
Thanks!


Sat Feb 28, 2015 10:56 am
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Question for MHTS
By the way, is there any way to reduce lag in teleop/make it more efficient? I want to try to get the least latency possible while making it neater.

_________________
Thanks!


Sun Mar 01, 2015 12:04 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Question for MHTS
akash329dsouza wrote:
Yes! We made it to super regionals in Iowa. We finished 9th place in robot runs. (We won 2nd place inspire!)

Congratulations! Wish you the best of luck.


Sun Mar 01, 2015 7:54 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Question for MHTS
akash329dsouza wrote:
By the way, is there any way to reduce lag in teleop/make it more efficient? I want to try to get the least latency possible while making it neater.

What do you mean by "lag"? Would you describe in more details? Do you mean your robot is not very responsive to joystick control?


Sun Mar 01, 2015 7:55 am
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Question for MHTS
Yes, we want to increase responsiveness in teleop to joystick control.

_________________
Thanks!


Sun Mar 01, 2015 12:04 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Question for MHTS
Would you post your teleop code? I can look it over and make some suggestions.


Sun Mar 01, 2015 1:31 pm
Profile
Novice

Joined: Tue Dec 16, 2014 10:25 am
Posts: 81
Post Re: Question for MHTS
So here is our Teleop Code:
Code:
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     Mux,            sensorI2CCustom)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S4,     Ultra,          sensorSONAR)
#pragma config(Motor,  mtr_S2_C1_1,     motorD,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     motorE,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     motorF,        tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     motorG,        tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C3_1,     motorH,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C4_1,    Ball_Reel_L,          tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C4_2,    Goal_Grab,            tServoStandard)
#pragma config(Servo,  srvo_S2_C4_3,    Ball_Reel_R,          tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C4_4,    Lift,                 tServoStandard)
#pragma config(Servo,  srvo_S2_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
int Sensitivity = 1;   //Set beginning sensitivity for controller
int DesiredAngle;   //Creates Desired Angle Variable
int speed2 = 0;
#include "HDStuff.h"
GYRO HDGyro;   //Create Gyro struct
const tMUXSensor leftIR = msensor_S1_2;
const tMUXSensor rightIR = msensor_S1_1;
const tMUXSensor frontUS = msensor_S1_4;
#include "Teleop.h"   //Teleop functions
task Controller_1()
{
   int Threshold = 10;   //Sets controller threshold
   int VLFinUse = 10;
   while(true){
      wait1Msec(30);
      if(joy1Btn(02) == 1){
         IRLineUp();
      }
      else if(joy1Btn(04) == 1){
         MoveForward(85,30);
      }
      else   if(joystick.joy1_TopHat == 0) //Up button on the D-Pad
      {
         Sensitivity = 1; //Sets the joystick sensitivity controlls to 1
      }
      else if(joystick.joy1_TopHat == 4) //Down button on the D-Pad
      {
         Sensitivity = 3; //Sets the joystick sensitivity controlls to 4
      }
      else if(joystick.joy1_TopHat == 6) //Up button on the D-Pad
      {
         Sensitivity = 3; //Sets the joystick sensitivity controlls to 3
      }
      else if(joystick.joy1_TopHat == 2) //Down button on the D-Pad
      {
         Sensitivity = 2; //Sets the joystick sensitivity controlls to 2
      }
      if(joystick.joy1_y1 > 100 && joystick.joy1_y2 > 100)   //If the left and right y axis are past 100.
      {
         if(VLFinUse == 0) //If VLF(Virtual line follower that uses the gyro with a preportional loop to follow a line) Was not running previously
         {
            DesiredAngle = HDGyro.heading; //Set the desired angle to the current heading
         }
         VLFinUse = 1; //Set VLFinUse to 1
         VLFforward();   //Turn on VLFforward
      }
      else if(joystick.joy1_y1 < -100 && joystick.joy1_y2 < -100) //If the left and right y axis are less than -100.
      {
         if(VLFinUse == 0) //If VLF was not in use
         {
            DesiredAngle = HDGyro.heading;   //Set desired angle to current heading
         }
         VLFinUse = 1;   //Vlf is set to in use
         VLFbackward();   //Turn on VLFbackward
      }
      else if(abs(joystick.joy1_y1) > Threshold || abs(joystick.joy1_y2) > Threshold)   //If joystick y1 is larger than threshold or joystick y2 is larger than threshold
      {
         VLFinUse = 0;   //VLF is not in use
         motor[motorF] = joystick.joy1_y1/Sensitivity;   //Set motorF to joystick joy1 devided by sensitivity
         motor[motorG] = joystick.joy1_y1/Sensitivity; //Set motorG to joystick joy1 devided by sensitivity
         motor[motorD] = joystick.joy1_y2/Sensitivity; //Set motorD to joystick joy2 devided by sensitivity
         motor[motorE] = joystick.joy1_y2/Sensitivity; //Set motorE to joystick joy2 devided by sensitivity
      }
      else   //If none of other conditions are met
      {
         Brake(); //Break
         VLFinUse = 0; //Set VLFinUse to 0
      }
   }
}

task Controller_2()
{
   while (true){
      wait1Msec(30);
      if(joy2Btn(07) == 1){
      BallReelStop();
      }
      else if(joy2Btn(08) == 1){
      BallReelStart();
      }
      else if(joy2Btn(04) == 1) //Up Center Goal
      {
         LiftGoal(2850,2800);
      }
      else if(joy2Btn(03) == 1) //Up Small Goal
      {
         LiftGoal(1235,1185);
      }
      else if(joy2Btn(02) == 1) //Up Medium
      {
         LiftGoal(1665,1615);
      }
      else if(joy2Btn(01) == 1) //Up Large
      {
         LiftGoal(2300,2250);
      }
      else if(joystick.joy2_y1 > 100 && joystick.joy2_y2 > 100)
      {
         ManualLiftUp();
      }
      else if(joystick.joy2_y1 < -100 && joystick.joy2_y2 < -100)
      {
         motor[motorH] = -2;
      }
      else if(joystick.joy2_TopHat == 0)
      {
         if(nMotorEncoder[motorH] < -3500)
         {
            servoChangeRate[Lift] = 8;
            servo[Lift] = 245;
         }
      }
      else{
         servoChangeRate[Lift] = 10;
         motor[motorH] = 0;
         servo[Lift] = 115;
      }
      if(joy2Btn(05) == 1)   //If joystick left button is pressed
      {
         servo[Goal_Grab] = 0;   //Move the ball hook to up position
      }
      else if(joy2Btn(06) == 1) //If joystick right button is pressed
      {
         servo[Goal_Grab] = 75;   //Move the ball hook to down position
      }
   }
}

task Failsafe()
{
   while(true)
   {
      wait1Msec(30);
      if(joy1Btn(10) == 1)
      {
         while(joy1Btn(10) == 1){
            wait1Msec(30);
         }
         motor[motorH] = 0;
         stopTask(Controller_2);
         wait1Msec(700);
         startTask(Controller_2);
      }
   }
}

task main()
{
   GyroInit(HDGyro,S3);   //Initilise gyro
   startTask(Failsafe);
   startTask(GyroUpdate);   //Start Gyro Update task to keep track of heading
   startTask(Controller_1);
   startTask(Controller_2);
   servo[Lift] = 115;
   while(true)   //While true (Forever while loop)
   {
      wait1Msec(30);   //Wait 30 milliseconds to reduce latency
      getJoystickSettings(joystick);   //Get the joystick settings
   }
}


Here is Teleop.h:
Code:
/*
This is our function library that holds all of our functions for Teleop.

*/
#pragma systemFile

float IRGetACDir(tMUXSensor link, int line)
{
   static float prevDir = 0.0; //Create variables
   float currDir;
   int acS[5];
   int idx;
   idx = HTIRS2readACDir(link);  //Read current zone and store in idx
   currDir = (float)idx; //Store current zone as a float instead of an integer
   if (idx == 0)// if HTIRS2readACDir returns 0, it means it lost the IR beacon. In that case we use the ACDir determined previously.
   {
      currDir = prevDir;
   }
   else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4])) //Read the strength values of all 5 sensors.
   {
      idx = (idx - 1)/2; //translate the zone value into the sensor number 0 to 4. For example, zone 4 yielded idx 1.
      if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0)) //if sensor number and sensor number + 1 are still within 0 through 4 and if sensor number and sensor number + 1 both have non-zero strength
      {
         // calculate the .x proportion value from the relative strength of the two adjacent sensors. The result could be a positive or a negative number smaller than 1.0. Then add this .x number to the original zone number from HTIRS2readACDir.
         currDir += (float)(acS[idx + 1] - acS[idx])/
         max(acS[idx], acS[idx + 1]);
      }
      nxtDisplayTextLine(line, "Idx=%d,Dir=%5.1f", idx, currDir);
   }
   prevDir = currDir; // remember the current zone value in case the next time we lost the IR beacon.

   return currDir;
}

void ManualLiftUp()
{
         displayTextLine(2, "Speed :  %4d", speed2);
         displayTextLine(3, "LiftEncoder :  %4d", nMotorEncoder[motorH]/4);
         speed2 = 25 + (abs(nMotorEncoder[motorH]/4)/200);
         motor[motorH] = speed2;
}

void BallReelStop()
{
   servo[Ball_Reel_L] = 127;   //Send servo commands to stop the ball reel
   servo[Ball_Reel_R] = 127;
}

void BallReelStart()
{
   servo[Ball_Reel_L] = 255;   //Send servo commands to start the ball reel
   servo[Ball_Reel_R] = 0;
}

void LiftGoal(int Up,int Down)
{
   nMotorEncoder[motorH] = 0;
   while(nMotorEncoder[motorH]/4 > -Up)
   {
      displayTextLine(2, "Speed :  %4d", speed2);
      displayTextLine(3, "LiftEncoder :  %4d", nMotorEncoder[motorH]/4);
      speed2 = 25 + (abs(nMotorEncoder[motorH]/4)/200);
      motor[motorH] = speed2;
   }
   motor[motorH] = 0;
   wait1Msec(600)
   while(joy2Btn(04) == 0)
   {
      motor[motorH] = 5;
      wait1Msec(30);
   }
   servoChangeRate[Lift] = 8;
   servo[Lift] = 240;
   while(joy2Btn(04) == 1)
   {
      motor[motorH] = 5;
      wait1Msec(30);
   }
   while(joy2Btn(04) == 0)
   {
      motor[motorH] = 5;
      wait1Msec(30);
   }
   nMotorEncoder[motorH] = 0;
   servo[Lift] = 115;
   servoChangeRate[Lift] = 10;
   while(nMotorEncoder[motorH]/4 < Down){
      displayTextLine(3, "Color :  %4d", nMotorEncoder[motorH]/4);
      motor[motorH] = -2;
   }
   motor[motorH] = 0;
}


void MoveForward(int degrees, int speed)//Move function
{
   nMotorEncoder[motorD] = 0;   //Reset Encoders
   nMotorEncoder[motorE] = 0;  //Reset Encoders
   nMotorEncoder[motorF] = 0;  //Reset Encoders
   nMotorEncoder[motorG] = 0;  //Reset Encoders
   {
      while (((nMotorEncoder[motorD] + nMotorEncoder[motorE] + nMotorEncoder[motorF] + nMotorEncoder[motorG])/4) < (degrees*4) || joy1Btn(03) == 1)   //While motor encoders are less than degrees we want to go
      {
         motor[motorD] = speed;   //Motors go at specified speed
         motor[motorE] = speed;  //Motors go at specified speed
         motor[motorF] = speed;  //Motors go at specified speed
         motor[motorG] = speed;  //Motors go at specified speed
      }
      motor[motorD] = 0;   //Set motor speed to 0
      motor[motorE] = 0;  //Set motor speed to 0
      motor[motorF] = 0;  //Set motor speed to 0
      motor[motorG] = 0;  //Set motor speed to 0
   }
}

int MotorCorrection; //Create MotorCorrection integer
task GyroUpdate //GyroUpdate task
{
   while(true){   //While true(Forever)
      wait1Msec(30);   //Wait 30 milliseconds for each loop
      GyroTask(HDGyro);   //Integrate turn rate into heading
   }
}

void VLFforward()   //VLF forward void
{
   int MotorSpeed = 100/Sensitivity;   //Motor speed is set at 100 devided by sensitivity
   int ProportionalGain = 3; //K- value
   MotorCorrection = ProportionalGain*(HDGyro.heading - DesiredAngle); //Calculates Motor Correction to make
   motor[motorD] = MotorSpeed + MotorCorrection; //Moves and applies motor correction (You may need to adjust this depending on if the sign is correct)
   motor[motorE] = MotorSpeed + MotorCorrection; //Moves and applies motor correction
   motor[motorF] = MotorSpeed - MotorCorrection; //Moves and applies motor correction
   motor[motorG] = MotorSpeed - MotorCorrection; //Moves and applies motor correction
}
void IRLineUp()
{
   int Speed = -30;
   int dist = USreadDist(frontUS);
   int Kp = 7;
   while (dist > 23 && joy1Btn(03) == 0)
   {
      dist = USreadDist(frontUS);
      float irDir = IRGetACDir(leftIR,1) + IRGetACDir(rightIR,2);
      float error = irDir - 10.0;
      nxtDisplayTextLine(3, "Error=%d,Dir=%5.1f", error, 1);
      motor[motorF] =Speed + (Kp*error);
      motor[motorG] = Speed + (Kp*error);
      motor[motorD] = Speed - (Kp*error);
      motor[motorE] = Speed - (Kp*error);
      wait1Msec(20);
   }
   motor[motorF] =0;
   motor[motorG] = 0;
   motor[motorD] = 0;
   motor[motorE] = 0;
}
void VLFbackward()   //VLF backward void
{
   int MotorSpeed = 100/Sensitivity;   //Motor speed is set at 100 devided by sensitivity
   int ProportionalGain = 3; //K- value
   MotorCorrection = ProportionalGain*(HDGyro.heading - DesiredAngle); //Calculates Motor Correction to make
   nxtDisplayCenteredBigTextLine(4, "%5.1f",HDGyro.heading); //Displays the current Gyro Heading
   motor[motorD] = -MotorSpeed + MotorCorrection; //Moves and applies motor correction (You may need to adjust this depending on if the sign is correct)
   motor[motorE] = -MotorSpeed + MotorCorrection; //Moves and applies motor correction
   motor[motorF] = -MotorSpeed - MotorCorrection; //Moves and applies motor correction
   motor[motorG] = -MotorSpeed - MotorCorrection; //Moves and applies motor correction
}

void Brake() //Brake motors
{
   motor[motorD] = 0;
   motor[motorE] = 0;
   motor[motorF] = 0;
   motor[motorG] = 0;
}

_________________
Thanks!


Sun Mar 01, 2015 6:12 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 32 posts ]  Go to page 1, 2, 3  Next

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.