 | Code: #pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, HTServo) #pragma config(Sensor, S1, , sensorI2CMuxController) #pragma config(Sensor, S2, Magnet, sensorAnalogInactive) #pragma config(Sensor, S3, touch, sensorTouch) #pragma config(Sensor, S4, gyro, sensorNone) #pragma config(Motor, mtr_S1_C1_1, LeftD, tmotorNormal, openLoop) #pragma config(Motor, mtr_S1_C1_2, RightD, tmotorNormal, openLoop, reversed) #pragma config(Motor, mtr_S1_C2_1, Collector, tmotorNormal, openLoop) #pragma config(Motor, mtr_S1_C2_2, Arm1, tmotorNormal, openLoop) #pragma config(Motor, mtr_S1_C3_1, Arm2, tmotorNormal, openLoop) #pragma config(Motor, mtr_S1_C3_2, Lift, tmotorNormal, openLoop) #pragma config(Servo, srvo_S1_C4_1, SpinGrip, tServoContinuousRotation) #pragma config(Servo, srvo_S1_C4_2, GangR, tServoStandard) #pragma config(Servo, srvo_S1_C4_3, GangL, tServoStandard) #pragma config(Servo, srvo_S1_C4_4, MagArm, tServoStandard) #pragma config(Servo, srvo_S1_C4_5, Bal1, tServoStandard) #pragma config(Servo, srvo_S1_C4_6, Bal2, tServoStandard) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
///////////////////////////////////////////////////////////////////////////////////////////////////// // // Tele-Operation Mode Code Template // // This file contains a template for simplified creation of an tele-op program for an FTC // competition. // // You need to customize two functions with code unique to your specific robot. // /////////////////////////////////////////////////////////////////////////////////////////////////////
#include "JoystickDriver.c" //Include file to "handle" the Bluetooth messages. //#include "drivers/HTMAG-driver.h" //include file to operate magnet sensor
///////////////////////////////////////////////////////////////////////////////////////////////////// // // initializeRobot // // Prior to the start of tele-op mode, you may want to perform some initialization on your robot // and the variables within your program. // // In most cases, you may not have to add any code to this function and it will remain "empty". // /////////////////////////////////////////////////////////////////////////////////////////////////////
void initializeRobot() { // Place code here to sinitialize servos to starting positions. // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
return ; }
///////////////////////////////////////////////////////////////////////////////////////////////////// // // Main Task // // The following is the main code for the tele-op robot operation. Customize as appropriate for // your specific robot. // // Game controller / joystick information is sent periodically (about every 50 milliseconds) from // the FMS (Field Management System) to the robot. Most tele-op programs will follow the following // logic: // 1. Loop forever repeating the following actions: // 2. Get the latest game controller / joystick settings that have been received from the PC. // 3. Perform appropriate actions based on the joystick + buttons settings. This is usually a // simple action: // * Joystick values are usually directly translated into power levels for a motor or // position of a servo. // * Buttons are usually used to start/stop a motor or cause a servo to move to a specific // position. // 4. Repeat the loop. // // Your program needs to continuously loop because you need to continuously respond to changes in // the game controller settings. // // At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program. // /////////////////////////////////////////////////////////////////////////////////////////////////////
int tweekArm2(int joyValue){ const int TWEEK =0; if(abs(joyValue)<5){ return 0; }
return 0; } int scaleMotor(int joyValue){ const int MAX_MOTOR= 100; const float MAX_JOY = 127.0; /*deadzone*/ if(abs(joyValue)<5){ return 0; } //calculate scale value easier to control robot int dir=joyValue/abs(joyValue); // gets neg or pos. float ratio = (joyValue*joyValue)/(MAX_JOY*MAX_JOY); int scale=(ratio*MAX_MOTOR)*dir; return scale; }
task drive() { while(true) { getJoystickSettings(joystick); //controls tank drive motor[LeftD]=scaleMotor(joystick.joy1_y1); motor[RightD]=scaleMotor(joystick.joy1_y2); } } //needs adjustmemts senses magnets and auto separates task magnet(){ while(true){ nxtDisplayTextLine(2, "Bias: %4d",SensorValue[Magnet]); if(SensorValue[Magnet]<508||/*SensorValue[Magnet]>700||*/SensorValue[Magnet]>515){ servo[MagArm]=250; motor[Collector]=25; wait1Msec(1050); motor[Collector]=0; wait1Msec(1500); servo[MagArm]=0; } } } task main() { initializeRobot(); waitForStart(); // wait for start of tele-op phase StartTask(drive); StartTask(magnet); while (true) { getJoystickSettings(joystick); motor[Arm1]=joystick.joy2_y1; motor[Arm2]=joystick.joy2_y1;
//collector if(joy1Btn(4)==1){ motor[Collector]=25; } else if(joy1Btn(3)){ motor[Collector]=0; } else if(joy1Btn(2)){ motor[Collector]=-100; } //normal ball depositor arm else if(joy2Btn(1)){ servo[Bal2]=250; wait1Msec(5000); servo[Bal2]=0; } //servos mag depositor arm else if(joy1Btn(1)){ servo[Bal1]=25; wait1Msec(5000); servo[Bal1]=0; } //lift else if(joy2Btn(2)){ while(SensorValue(touch)!=1){ motor[Lift]=100; }
} else if(joy2Btn(2)==0){ motor[Lift]=0; } else if (joy2Btn(5)){ motor[Lift]=-100; } //gripper servos else if(joy2Btn(8)){ servo[GangR]=200; servo[GangL]=-200; } //flipper clock rotate joy2 ****tweeking needed else if(joy2Btn(6)){ servo[SpinGrip]=255; } //flipper stops joy2 ***** else if(joy2Btn(6)==0){ servo[SpinGrip]=127; } //flipper rotates Counter joy2 **** else if(joy2Btn(9)){ servo[SpinGrip]=0; } else if(joy2Btn(9)==0){ servo[SpinGrip]=127; }
/////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////// //// //// //// Add your robot specific tele-op code here. //// //// //// /////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////
// Insert code to have servos and motors respond to joystick and button values.
// Look in the ROBOTC samples folder for programs that may be similar to what you want to perform. // You may be able to find "snippets" of code that are similar to the functions that you want to // perform. } }
|  |