Example #1
0
	void initRobot () {
			cerr << "running init\n";
			Dlf->EnableControl(0);
			Dlb->EnableControl(0);
			Drf->EnableControl(0);
			Drb->EnableControl(0);
			arm1->EnableControl();
			arm1_sec->EnableControl();
			arm2->EnableControl();
			
			
			
			Dlf->ConfigEncoderCodesPerRev(250);
			Dlf->SetPID(1,0,0);
			Dlb->ConfigEncoderCodesPerRev(250);
			Dlb->SetPID(1,0,0);
			Drf->ConfigEncoderCodesPerRev(250);
			Drf->SetPID(1,0,0);
			Drb->ConfigEncoderCodesPerRev(250);
			Drb->SetPID(1,0,0);
			Wait(.1);
			if(robotInted==false) {
				int count=220;
				arm2->Set(-.3);
				while(count-->0 && LimitClaw.Get() == 1) Wait(.005);
				arm2->Set(.15);
				while(count-->0 && LimitClaw.Get() == 0) Wait(.005);
				arm2->Set(0);
				if(count>0)
					EncClaw.Reset();
				arm1->Set(-.3);
				arm1_sec->Set(-.3);
				while(count-->0 && LimitArm.Get() == 1) Wait(.005);
				arm1->Set(.5);
				arm1_sec->Set(.5);
				while(count-->0 && LimitArm.Get() == 0) Wait(.005);
				if(count>0)
					EncArm.Reset();
				arm1->Set(0);
				arm1_sec->Set(0);
				
				robotInted = true;
			}
		}
Example #2
0
	void RawControl::resetArm()
	{
		arm->SetPID(-5, 0, 0);
		arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		arm->SetSafetyEnabled(false);
		arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
		arm->ConfigEncoderCodesPerRev(1);
		arm->EnableControl(0);
		
	}
void MecanumDrive::InitMotor( CANJaguar& motor )
{
	motor.ChangeControlMode( m_currControlMode );
	if ( m_currControlMode == CANJaguar::kSpeed )
	{
		motor.ConfigEncoderCodesPerRev(360);
		motor.ConfigMaxOutputVoltage(12.0);
		motor.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		motor.SetPID(.7,.004,0);
		motor.SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
	}
	motor.EnableControl();
}
Example #4
0
	void RawControl::checkJag()
	{
		
		if(frontRight->GetFaults()!=0)
		{
			frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			frontRight->SetSafetyEnabled(false);
			frontRight->ConfigEncoderCodesPerRev(360);
			frontRight->EnableControl(0);

		}
		if(frontLeft->GetFaults()!=0)
		{
			frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			frontLeft->SetSafetyEnabled(false);
			frontLeft->ConfigEncoderCodesPerRev(360);
			frontLeft->EnableControl(0);
			
		}
		if(backLeft->GetFaults()!=0)
		{
			backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			backLeft->SetSafetyEnabled(false);
			backLeft->ConfigEncoderCodesPerRev(360);
			backLeft->EnableControl(0);
			
		}
		if(backRight->GetFaults()!=0)
		{
			backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			backRight->SetSafetyEnabled(false);
			backRight->ConfigEncoderCodesPerRev(360);
			backRight->EnableControl(0);
			
		}
		if(arm->GetFaults()!=0)
		{
			arm->SetPID(ARM_P, -.02, 0);
			arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
			arm->SetSafetyEnabled(false);
			arm->ConfigMaxOutputVoltage(13);
			arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
			arm->ConfigPotentiometerTurns(1);
			arm->EnableControl(0);
		}
		
		
	}
Example #5
0
//CHECK THIS OUT!!
	void RawControl::configJags() {
		//will need to be tuned on the new robot
		frontLeft->SetPID(3, .07, 0);//tested values are 1,.02,0
		frontRight->SetPID(3, .09, 0);
		backLeft->SetPID(1, .013, 0);
		backRight->SetPID(1.2, .013, 0);
		arm->SetPID(ARM_P, -.02, 0);

		backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		//shoot everything remotely safety related
		backLeft->SetSafetyEnabled(false);
		backRight->SetSafetyEnabled(false);
		frontLeft->SetSafetyEnabled(false);
		frontRight->SetSafetyEnabled(false);
		arm->SetSafetyEnabled(false);
		
		
		

		frontLeft->ConfigMaxOutputVoltage(13);
		frontRight->ConfigMaxOutputVoltage(13);
		backLeft->ConfigMaxOutputVoltage(13);
		backRight->ConfigMaxOutputVoltage(13);
		arm->ConfigMaxOutputVoltage(13);

		frontLeft->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		frontRight->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		backRight->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		backLeft->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
		//not sure on these values either
		frontLeft->ConfigEncoderCodesPerRev(360);
		frontRight->ConfigEncoderCodesPerRev(360);
		backLeft->ConfigEncoderCodesPerRev(360);
		backRight->ConfigEncoderCodesPerRev(360);
		arm->ConfigPotentiometerTurns(1);
		frontLeft->ChangeControlMode(CANJaguar::kPercentVbus);
		frontRight->ChangeControlMode(CANJaguar::kPercentVbus);
		backLeft->ChangeControlMode(CANJaguar::kPercentVbus);
		backRight->ChangeControlMode(CANJaguar::kPercentVbus);
		frontLeft->EnableControl(0);
		frontRight->EnableControl(0);
		backLeft->EnableControl(0);
		backRight->EnableControl(0);
		frontLeft->ConfigFaultTime(0);
		backLeft->ConfigFaultTime(0);
		backRight->ConfigFaultTime(0);
		frontRight->ConfigFaultTime(0);
		arm->ConfigFaultTime(0);
		arm->EnableControl(0);
		arm->EnableControl(0);
		
		
		/*
		 fl=new CANJaguar(2,CANJaguar::kSpeed);
		 fr=new CANJaguar(3,CANJaguar::kSpeed);
		 bl=new CANJaguar(4,CANJaguar::kSpeed);
		 br=new CANJaguar(1,CANJaguar::kSpeed);
		 
		 fl->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		 fr->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		 br->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		 bl->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		 
		 fl->ConfigEncoderCodesPerRev(1440);
		 fr->ConfigEncoderCodesPerRev(1440);
		 bl->ConfigEncoderCodesPerRev(1440);
		 br->ConfigEncoderCodesPerRev(1440);
		 */
	}
Example #6
0
    void RunWheels()
    {
//	uint32_t t0, t1, t2, t3;

	// schedule updates to avoid overloading CAN bus or CPU
	switch (report++) {
	case 12:		// 240 milliseconds
	    report = 0;		// reset counter
	case 0:
	    // Update PID parameters
	    double newP = SmartDashboard::GetNumber("Shooter P");
	    double newI = SmartDashboard::GetNumber("Shooter I");
	    double newD = SmartDashboard::GetNumber("Shooter D");
	    if (newP != kP || newI != kI || newD != kD) {
		kP = newP;
		kI = newI;
		kD = newD;
#ifdef HAVE_TOP_WHEEL
		if (topPID) {
#ifdef HAVE_TOP_CAN1
		    ; // topWheel1->SetPID( kP, kI, kD );
#endif
#ifdef HAVE_TOP_CAN2
		    topWheel2->SetPID( kP, kI, kD );
#endif
		}
#endif
#ifdef HAVE_BOTTOM_WHEEL
		if (bottomPID) {
#ifdef HAVE_BOTTOM_CAN1
		    ; // bottomWheel1->SetPID( kP, kI, kD );
#endif
#ifdef HAVE_BOTTOM_CAN2
		    bottomWheel2->SetPID( kP, kI, kD );
#endif
		}
#endif
	    }
	    break;

	case 4:			// 80 milliseconds
#ifdef HAVE_TOP_WHEEL
//t0 = GetFPGATime();
	    // Get top output voltage, current and measured speed
#ifdef HAVE_TOP_CAN1
	    double topI1 = topWheel1->GetOutputCurrent();
#endif
#ifdef HAVE_TOP_CAN2
	    double topI2 = topWheel2->GetOutputCurrent();
	    topJagSpeed  = topWheel2->GetSpeed(); 
#endif
//t1 = GetFPGATime();
	    topTachSpeed = topTach->PIDGet();

#ifdef HAVE_TOP_CAN1
	    // stupid floating point!
	    Log(LOG_CURRENT, 1, (uint32_t)(topI1 * 1000 + 0.5));
#endif
#ifdef HAVE_TOP_CAN2
	    Log(LOG_CURRENT, 2, (uint32_t)(topI2 * 1000 + 0.5));
	    Log(LOG_SPEED,   2, (uint32_t)(topJagSpeed + 0.5));
#endif

	    // Send values to SmartDashboard
#ifdef HAVE_TOP_CAN1
	    SmartDashboard::PutNumber("Top Current 1", topI1);
#endif
#ifdef HAVE_TOP_CAN2
	    SmartDashboard::PutNumber("Top Current 2", topI2);
	    SmartDashboard::PutNumber("Top Jag      ", topJagSpeed);
#endif
	    SmartDashboard::PutNumber("Top Tach     ", topTachSpeed);

	    // Get setpoint
	    topSpeed = SmartDashboard::GetNumber("Top Set      ");
//t2 = GetFPGATime();

	    if (spinFastNow) {
		if (topPID) {
		    if (topJagSpeed < topSpeed * vbusThreshold) {
			topPID = false;
			// below threshold: switch both motors to full output
#ifdef HAVE_TOP_CAN1
			jagVbus(topWheel1, maxOutput);
			Log(LOG_MODE, 1, 1);
#endif
#ifdef HAVE_TOP_PWM1
			topWheel1->Set(maxOutput);
			Log(LOG_MODE, 1, 1);
#endif
#ifdef HAVE_TOP_CAN2
			jagVbus(topWheel2, maxOutput);
			Log(LOG_MODE, 2, 1);
#endif
		    } else {
			; // above threshold: run motor 1 off, PID on motor 2
#ifdef HAVE_TOP_CAN1
			topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_PWM1
			topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_CAN2
			topWheel2->Set(topSpeed);
#endif
		    }
		} else {
		    if (topJagSpeed >= topSpeed * pidThreshold) {
			; // above threshold: switch motor 1 off, motor 2 PID
			topPID = true;
#ifdef HAVE_TOP_CAN1
			topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_PWM1
			topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_CAN2
			jagPID(topWheel2, topSpeed);
			Log(LOG_MODE, 2, 2);
#endif
		    } else {
			; // below threshold: run both motors at full output
#ifdef HAVE_TOP_CAN1
			topWheel1->Set(maxOutput);
#endif
#ifdef HAVE_TOP_PWM1
			topWheel1->Set(maxOutput);
#endif
#ifdef HAVE_TOP_CAN2
			topWheel2->Set(maxOutput);
#endif
		    }
		}
//t3 = GetFPGATime();
//printf("%10u %10u %10u\n", (uint32_t)(t1 - t0), (uint32_t)(t2 - t1), (uint32_t)(t3 - t2));
	    }
#endif

	    break;

	case 8:		// 160 milliseconds
#ifdef HAVE_BOTTOM_WHEEL
	    // Get bottom output voltage, current and measured speed
//t0 = GetFPGATime();
#ifdef HAVE_BOTTOM_CAN1
	    double bottomI1 = bottomWheel1->GetOutputCurrent();
#endif
#ifdef HAVE_BOTTOM_CAN2
	    double bottomI2 = bottomWheel2->GetOutputCurrent();
	    bottomJagSpeed  = bottomWheel2->GetSpeed();
#endif
//t1 = GetFPGATime();
	    bottomTachSpeed = bottomTach->PIDGet();

#ifdef HAVE_BOTTOM_CAN1
	    Log(LOG_CURRENT, 3, (uint32_t)(bottomI1 * 1000 + 0.5));
#endif
#ifdef HAVE_BOTTOM_CAN2
	    Log(LOG_CURRENT, 4, (uint32_t)(bottomI2 * 1000 + 0.5));
	    Log(LOG_SPEED,   4, (uint32_t)(bottomJagSpeed + 0.5));
#endif

	    // Send values to SmartDashboard
#ifdef HAVE_BOTTOM_CAN1
	    SmartDashboard::PutNumber("Bottom Current 1", bottomI1);
#endif
#ifdef HAVE_BOTTOM_CAN2
	    SmartDashboard::PutNumber("Bottom Current 2", bottomI2);
	    SmartDashboard::PutNumber("Bottom Jag      ", bottomJagSpeed);
#endif
	    SmartDashboard::PutNumber("Bottom Tach     ", bottomTachSpeed);

	    // Get setpoint
	    bottomSpeed = SmartDashboard::GetNumber("Bottom Set      ");
//t2 = GetFPGATime();

	    if (spinFastNow) {
		if (bottomPID) {
		    if (bottomJagSpeed < bottomSpeed * vbusThreshold) {
			bottomPID = false;
			// below threshold: switch both motors to full output
#ifdef HAVE_BOTTOM_CAN1
			jagVbus(bottomWheel1, maxOutput);
			Log(LOG_MODE, 3, 1);
#endif
#ifdef HAVE_BOTTOM_PWM1
			bottomWheel1->Set(maxOutput);
			Log(LOG_MODE, 3, 1);
#endif
#ifdef HAVE_BOTTOM_CAN2
			jagVbus(bottomWheel2, maxOutput);
			Log(LOG_MODE, 4, 1);
#endif
		    } else {
			; // above threshold: run motor 1 off, PID on motor 2
#ifdef HAVE_BOTTOM_CAN1
			bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_PWM1
			bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
			bottomWheel2->Set(bottomSpeed);
#endif
		    }
		} else {
		    if (bottomJagSpeed >= bottomSpeed * pidThreshold) {
			// above threshold: switch motor 1 off, motor 2 PID
			bottomPID = true;
#ifdef HAVE_BOTTOM_CAN1
			bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_PWM1
			bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
			jagPID(bottomWheel2, bottomSpeed);
			Log(LOG_MODE, 4, 2);
#endif
		    } else {
			; // below threshold: run both motors at full output
#ifdef HAVE_BOTTOM_CAN1
			bottomWheel1->Set(maxOutput);
#endif
#ifdef HAVE_BOTTOM_PWM1
			bottomWheel1->Set(maxOutput);
#endif
#ifdef HAVE_BOTTOM_CAN2
			bottomWheel2->Set(maxOutput);
#endif
		    }
		}
//t3 = GetFPGATime();
//printf("%10u %10u %10u\n", (uint32_t)(t1 - t0), (uint32_t)(t2 - t1), (uint32_t)(t3 - t2));
	    }
#endif
	    break;
	}
    }
Example #7
0
	// Shamelessly stolen from PCVideoServer
	int DashboardCommandServer()
	{
	    /* Setup to PC sockets */
	    struct sockaddr_in serverAddr;
	    int sockAddrSize = sizeof(serverAddr);
	    int pcSock = ERROR;
	    bzero ((char *) &serverAddr, sockAddrSize);
	    serverAddr.sin_len = (u_char) sockAddrSize;
	    serverAddr.sin_family = AF_INET;
	    serverAddr.sin_port = htons (kDashboardCommandPort);
	    serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);

	    while (true)
	    {
	        taskSafe();
	    	  //  Create the socket.
	        if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
	        {
	            perror ("socket");
	            continue;
	        }
	        //  Set the TCP socket so that it can be reused if it is in the wait state.
	        int reuseAddr = 1;
	        setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
	        //  Bind socket to local address.
	        if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
	        {
	            perror ("bind");
	            close (pcSock);
	            continue;
	        }
	        //  Create queue for client connection requests.
	        if (listen (pcSock, 1) == ERROR)
	        {
	            perror ("listen");
	            close (pcSock);
	            continue;
	        }

	        struct sockaddr_in clientAddr;
	        int clientAddrSize;
	        int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
	        if (newPCSock  == ERROR)
	        {
	            close(pcSock);
	            continue;
	        }

	        char cmdBuffer[32];
	        char *pBuffer;
	        while(1)
	        {
	            int numBytes = 0;
	            pBuffer = cmdBuffer;
	            while (numBytes < 2 || (*(pBuffer-2) != '\r' && *(pBuffer-1) != '\n'))
	            {
		            numBytes += read(newPCSock, pBuffer++, 1);
	            }
	            char command = cmdBuffer[0];
	            switch (command)
	            {
	            case 'E':
	            	speedJag.EnableControl();
	            	//printf("Enable\n");
	            	break;
	            case 'D':
	            	speedJag.DisableControl();
	            	//printf("Disable\n");
	            	break;
	            case 'G':
	            	{
	            		double P, I, D;
	            		memcpy((char*)&P, cmdBuffer+1, sizeof(double));
	            		memcpy((char*)&I, cmdBuffer+9, sizeof(double));
	            		memcpy((char*)&D, cmdBuffer+17, sizeof(double));
	            		speedJag.SetPID(P, I, D);
	            		//printf("Set- P: %f I: %f D: %f\n", P, I, D);
	            		//P = speedJag.GetP();
	            		//I = speedJag.GetI();
	            		//D = speedJag.GetD();
	            		//printf("Get- P: %f I: %f D: %f\n", P, I, D);
	            	}
	            	break;
	            }

	            //no point in running too fast -
	            Wait(0.01);
	        }
	        //  Clean up
	        close (newPCSock);
	        newPCSock = ERROR;
	        close (pcSock);
	        pcSock = ERROR;
	        taskUnsafe();
	        Wait(0.1);
	    }
	    return (OK);
	}