int PhoBot::control(String command) {
// "F-100" - forward B, L, R
String action = command.substring(0, 1);
String speedStr = command.substring(2);
float speed = 0;
if (! action.equalsIgnoreCase("S")) {
speed = speedStr.toInt() / 100.0;
}
if (action.equalsIgnoreCase("F")) {
setMotor(M3, FORWARD, speed);
setMotor(M4, FORWARD, speed);
}
else if (action.equalsIgnoreCase("B")) {
setMotor(M3, BACK, speed);
setMotor(M4, BACK, speed);
}
else if (action.equalsIgnoreCase("L")) {
setMotor(M3, FORWARD, speed);
setMotor(M4, BACK, speed);
}
else if (action.equalsIgnoreCase("R")) {
setMotor(M3, BACK, speed);
setMotor(M4, FORWARD, speed);
}
else if (action.equalsIgnoreCase("S")) {
setMotor(M3, STOP);
setMotor(M4, STOP);
}
return 1;
}
TSensorModes
/*
* Spooler_10-21.c ~
* Alex Jones for team 5105 Buffalo Wings
*
* Description: a simple unit test for the spooler that Raj has created
*
* Uses: to test the gear config on the test equipment.
*
* Dependancies: NONE
*
* ~licensed under the MIT license included in this repositiory
*/
#include "JoystickDriver.c"
#include "../motors/SetMotor.fn"
#include "../controller/GetJoystick.fn"
task main()
{
while(true)
{
getJoystickSettings(joystick);
setMotor(spoolLeft, getJoystick(1, 1, 'y')/1.28);
setMotor(spoolRight, getJoystick(1, 1, 'y')/1.28);
}
}
void roboControl(void){ //For autonomous control
setLauncherSpeed(30);
launcherSpeed_new = 127;//Lets start this shoot speed
goForwardFor_time(10);//Go forward 10 seconds
goBackwardFor_time(1);
clearTimer(T1);
while(time1[T1] < 20000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}
/*goBackwardFor_time(1);
turnRight(0.1);
setMotor(hozBelt,-100);
goForwardFor_time(.5);
turnRight(0.1);
goForwardFor_time(5);*/
goForwardFor_time(7);//Go forward 7 seconds
goBackwardFor_time(1); //Get to launching distance
clearTimer(T1);
while(time1[T1] < 15000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}
goBackwardFor_time(1);
turnRight(1);
setMotor(hozBelt,-100);
goForwardFor_time(.5);
turnRight(0.25);
goForwardFor_time(5);
}
void turnLeft(int time)
{
resetTimer();
setMotor(MOTOR_A, -100);
setMotor(MOTOR_B, -100);
while (time1() < time);
}
/*Part 1*/
int ReadEncoding (){
int encodingArr[4];
int encodedValue = 0;
int i =0;
while (i<4){
nxtDisplayCenteredTextLine(2, "%d", SensorValue[lightSensor]);
if (SensorValue[lightSensor]<=BLACK_COLOUR){
encodingArr[i] = 1;
}
else{
encodingArr[i] = 0;
}
if(i!=3){
/* calibrate to 7 cm*/
setMotor(50);
wait1Msec(330);
}
setMotor(0);
wait1Msec(1000);
i++;
}
setMotor(0);
i = 0;
int increment = 8;
while (i<4){
if (encodingArr[i])
encodedValue+= increment;
increment = increment/2;
i++;
}
nxtDisplayCenteredTextLine(2, "%d", encodedValue);
return (encodedValue);
}
void turnRight(int time)
{
resetTimer();
setMotor(MOTOR_A, 100);
setMotor(MOTOR_B, 100);
while (time1() < time);
}
// Großer Wert = Zu
void curtain_setPosition(uint8_t pos)
{
// Do not accept new positions if the module is currently reseting to the start position
// or the sensor is not working or pos is an invalid value
if (state == SensorNotWorkingState || state == SensorCalibratingState)
return;
// apply maximum value
if (pos > CURTAIN_COMPLETE_SENSOR_CHANGES)
pos = CURTAIN_COMPLETE_SENSOR_CHANGES;
setMotor(0, DirectionDown);
movetoposition = currentposition;
// Down: high value
if (pos > currentposition) {
state = DownState;
movetoposition = pos;
setMotor(255, DirectionDown);
} else if (pos < currentposition) { // Up: low value
movetoposition = pos;
if (currentposition-pos>2) {
state = FastUpState;
setMotor(255, DirectionUp);
}
else {
state = UpState;
setMotor(60, DirectionUp);
}
}
}
void IntakeSystem::intake(InputMethod* input) {
//auto intake button
if(input->intakeToggle()){
setMotor(1);
open();
isToggled = true;
}else if(isToggled && !input->intakeToggle()){
setMotor(0);
close();
isToggled = false;
}
//manual control of motors
if (!isToggled) {
if (input->intake())
setMotor(1);
else if (input->outtake())
setMotor(-1);
else
setMotor(0);
}
//catch ball
if(!catchToggle && input->catchBall()){
if(lift->Get() == Relay::kForward){
lift->Set(Relay::kReverse);
}else{
lift->Set(Relay::kForward);
}
}
catchToggle = input->catchBall();
}
void OrbitPlanet(int planetValue)
{
if (planetValue == 13){
// Find the planet
PlanetSearch(1);
wait1Msec(100); //// REMOVE THIS AFTER
// Do the square orbit
SquareOrbit(-1);
}
else if (planetValue == 9){
PlanetSearch(-1);
wait1Msec(100); //// REMOVE THIS AFTER
SquareOrbit(1);
}
else{
SearchSaturn();
motor[motorB] = 10;
motor[motorC] = -10;
wait1Msec(2000);
while(SensorValue[sonarSensor] > 60){
// Keep rotating
}
setMotor(0);
// Now pointing right at the planet, approach the planet
while(SensorValue[sonarSensor] > 15){
setMotor(50);
}
setMotor(0);
SquareOrbit(1);
}
}
void __attribute__((__interrupt__, auto_psv)) _IC2Interrupt(void)
{
_IC2IF = 0; // Clear the interrupt flag
//-------------------------------------------------------------------------
// If we are looking at the beginning (rising edge) of the pulse...
//-------------------------------------------------------------------------
if (IC2_PULSE_STATE == RISE)
{
IC2_PULSE_STATE = FALL; // Next interrupt occurs on falling edge
}
//-------------------------------------------------------------------------
// ... else we are looking at the end (falling edge) of the pulse.
//-------------------------------------------------------------------------
else
{
IC2_PULSE_STATE = RISE; // Next interrupt occurs on rising edge
++IC2_COUNT; // increase the sample count
if(!useCompass){
if(IC2_COUNT>=IC_GO_COUNTS || IC1_COUNT>=IC_GO_COUNTS){
currentMotorSetting=0;
setMotor(currentMotorSetting);
}//endif
}//endif
else{
if(f_Heading){
currentMotorSetting=0;
setMotor(currentMotorSetting);
}//endif
}//endelse
}
}//endic2
void roboControl(void){ //For autonomous control //This is test code
setLauncherSpeed(30);
launcherSpeed_new = 127;//Lets start this
goForwardFor_time(5.3,3);//Go forward 6.1 seconds
clearTimer(T1);
while(time1[T1] < 20000){setMotor(vertBelt,80);setMotor(hozBelt,100);
updateLauncherSpeed(2);
}
}
void PicoBorgRev::setMotors(float power)
{
if (power < 0) {
power = power * -1;
setMotor(PBR_COMMAND_SET_ALL_REV, power);
} else {
setMotor(PBR_COMMAND_SET_ALL_FWD,power);
}
}
/*lint -save -e970 Disable MISRA rule (6.3) checking. */
int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */
{
/* Write your local variable definition here */
uint16 countServo;
int i;
/*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
PE_low_level_init();
ADC0_CFG2 |= ADC_CFG2_MUXSEL_MASK;
/*** End of Processor Expert internal initialization. ***/
/* Write your code here */
/* For example: for(;;) { } */
/*
* Motores são setados com valores de 0 a 255, 2o argumento resolve o sentido.
*/
cameraSIBit_ClrVal();
sMachine_Init();
Serial_Init();
enableMotor();
setMotor(0,1,0);
setMotor(1,1,0);
servoPWM_Enable(servoPWM_DeviceData);
countServo = 19000;
servoPWM_SetDutyUS(servoPWM_DeviceData,countServo);
cameraCLK_Enable();
cameraCLK_EnableEvent();
cameraCLKBit_ClrVal();
while(1){
for (i=0; i < 128; i++){
vetorCamera[i] = getCamPixel(i);
}
for (i=0; i < 128; i++){
itoa(vetorCamera[i], buffer0);
Send_String(buffer0);
Send_String(" ");
}
Send_String("\r\n");
}
/*** Don't write any code pass this line, or it will be deleted during code generation. ***/
/*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
#ifdef PEX_RTOS_START
PEX_RTOS_START(); /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
#endif
/*** End of RTOS startup code. ***/
/*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
for(;;){}
/*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/
/*
void goForwardFor_time(int time){ //Goes forward for a set ammount of time
setMotor(leftMotor,autoSpeed);//Use -8 to go forward
setMotor(rightMotor,(autoSpeed-5));
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();setMotor(hozBelt,100);}
stopMotor(leftMotor);
stopMotor(rightMotor);
}
*/
void goForwardFor_time(int time, int launcherSpeedInc){ //Goes forward for a set ammount of time
setMotor(leftMotor,autoSpeed);
setMotor(leftMotor2,autoSpeed);
setMotor(rightMotor,autoSpeed);
setMotor(rightMotor2,autoSpeed);//Use -8 to go forward
setRightMotors(autoSpeed-20);
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();setMotor(hozBelt,100);}
stopLeftMotors();
wait(.8);
stopRightMotors();
}
static void cmdMotor(BaseSequentialStream *chp, int argc, char *argv[])
{
(void)argv;
uint8_t motor;
motor_direction_t direction;
motor_speed_t speed;
if (argc > 0) {
if (strcmp(argv[0], "on") == 0) {
setMotor(MOTOR_LEFT, motor_forward, 100);
setMotor(MOTOR_RIGHT, motor_forward, 100);
return;
} else if (strcmp(argv[0], "off") == 0) {
setMotor(MOTOR_LEFT, motor_stop, 0);
setMotor(MOTOR_RIGHT, motor_stop, 0);
return;
} else if ((argc > 1)) {
motor = atoi(argv[0]);
speed = 100;
switch (argv[1][0]) {
case 'f':
direction = motor_forward;
break;
case 'r':
direction = motor_reverse;
break;
default:
direction = motor_stop;
}
if (argc > 2) {
speed = atoi(argv[2]);
}
if (motor < 2) {
chprintf(chp, "Set motor %d to direction %d and speed %d\r\n", motor, direction, speed);
setMotor(motor, direction, speed);
return;
}
}
}
// Usage
chprintf(chp, "Usage: motor N <f|b|s> [speed]\r\n");
chprintf(chp, " motor 0 forward 50\r\n");
chprintf(chp, " motor 1 b\r\n");
chprintf(chp, " motor off\r\n");
chprintf(chp, "where N is motor number, 0-1\r\n");
chprintf(chp, "and direction f, b or s (forward, backward, stop)\r\n");
chprintf(chp, "and speed is percentage 0-100\r\n");
}
void driveForward(unsigned char speed_setting)
{
unsigned char speed;
switch (speed_setting) {
case MAX_SPEED_SETTING:
speed = MAX_SPEED;
break;
default:
speed = 0;
}
setMotor(leftMotorPort, speed + OFFSET);
setMotor(rightMotorPort, -speed + OFFSET);
}
void turnRight(int time){
setMotor(leftMotor,autoSpeed);
setMotor(leftMotor2,autoSpeed);
clearTimer(T1);
wait(time);
//while(time1[T1] < time * 1000){updateLauncherSpeed();}
//By calling to update the wheel speed, we lose accuracy.
//This is why I have changed it to a wait. Though we do not manage the speed control,
//we only lose it for a few seconds.
wait(time);
stopLeftMotors();
stopRightMotors();
}
void goForwardFor_time(int time){ //Goes forward for a set ammount of time
int driftCorrection = 40;
//setMotor(leftMotor,127);
setMotor(leftMotor2,127);
//setMotor(rightMotor,100);
setMotor(rightMotor2,127);
//setMotor(rightMotor,autoSpeed - driftCorrection);
//setMotor(rightMotor2,autoSpeed - driftCorrection);//Use -8 to go forward
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();setMotor(hozBelt,100);}
stopRightMotors();
stopLeftMotors();
}
void MoveForward (){
/* Black is less than 25*/
nxtDisplayCenteredTextLine(2, "%d", SensorValue[lightSensor]);
while (SensorValue[lightSensor] > BLACK_COLOUR){
nxtDisplayCenteredTextLine(2, "%d", SensorValue[lightSensor]);
setMotor(50);
wait1Msec(50);
}
//found BLACK
setMotor(0);
wait1Msec(1000);
}
void motor(void)
{
initialize();
wait1Msec(200);
//Drive Forward
setMotor(MOTOR_A, (50*0.95));
setMotor(MOTOR_B, 50);
wait1Msec(30000);
//Test Right Motor
setMotor(MOTOR_A, 50);
setMotor(MOTOR_B, 0);
wait1Msec(3000);
//Test Left Motor
setMotor(MOTOR_A, 0);
setMotor(MOTOR_B, 50);
wait1Msec(3000);
//Turn Motors Off
setMotor(MOTOR_A, 0);
setMotor(MOTOR_B, 0);
}
task main()
{
robotType(recbot); // We are using the recbot.
setMotor(port6, 63); // Set motor port 6 to speed 63.
wait(0.5); // Wait 0.5 seconds.
stopMotor(port6); // Stop motor port 6.
wait(1.0); // Wait 1.0 seconds.
setMotor(port6, -63); // Set motor port 6 to speed -63.
wait(0.5); // Wait 0.5 seconds.
stopMotor(port6); // Stop motor port 6.
}
void goBackwardFor_time(int time){ //Goes Backwards for a set ammount of time
//setMotor(leftMotor,-autoSpeed-8);
//setMotor(leftMotor2,-autoSpeed-8);
//setMotor(rightMotor,-autoSpeed);
//setMotor(rightMotor2,-autoSpeed);//Use -8 to go forward
setMotor(leftMotor,-50);
setMotor(leftMotor2,-50);
setMotor(rightMotor,-50);
setMotor(rightMotor2,-50);//Use -8 to go forward
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();}
stopLeftMotors();
stopRightMotors();
}
void main() {
initialize();
// Safety precaution - ensure car is not moving yet.
setMotor(MOTOR_A, 0);
setMotor(MOTOR_B, 0);
// Wait for button press to start.
while (getSensor(BUTTON) == 0);
while (getSensor(BUTTON) == 1);
// Modify this value for each course.
int courseNumber = 1;
drive(courseNumber);
}
void main (void)
{
initialize();
wait1Msec(200);
//initialize values
int pwr = 15;
int sensorState = -1;
//determine these values by running the calibration program first
int threshold1 = 150;
int threshold2 = 150;
int delay = 10;
//initialize motors and LED indicators
setMotor (MOTOR_A, 0);
setMotor (MOTOR_B, 0);
LEDnum(0);
LEDoff(RED_LED);
LEDoff(GREEN_LED);
//waits for a button press
while(getSensor(BUTTON) == 0);
wait1Msec(100);
while(getSensor(BUTTON) == 1);
resetTimer();
while(getSensor(BUTTON)== 0 && time100() < 64) {
//line follows for 6.4 seconds
if (sensorState != getSensorState(threshold1, threshold2)) {
//only change direction if conditions are different than before.
sensorState = getSensorState(threshold1,threshold2);
if (sensorState == 0) { //go straight
drive(15, delay);
} else if (sensorState == 1){
rightTurn(15,delay);
} else if (sensorState == 2){
leftTurn(15, delay);
} else if (sensorState == 3){
rightTurn(15, delay);
}
}
}
//hard code exit 2
rightTurn(pwr,2400);
drive(pwr,9400);
leftTurn(pwr,1500);
drive(pwr,20000);
}
task usercontrol()
{
word button5U2Last,
button5D2Last;
// User control code here, inside the loop
while (true)
{
arcade4(vexRT[Ch1], vexRT[Ch3], flWheel, blWheel, frWheel, brWheel);
joyDigiMtr2(roller, vexRT[Btn6U], 127, vexRT[btn6D], -127);
setMotor(elevator, vexRT[Ch2Xmtr2]);
if(vexRT[Btn5UXmtr2] && !button5U2Last) {
flyTarget += 5;
}
else if(vexRT[Btn5DXmtr2] && !button5D2Last) {
flyTarget -= 5;
}
if(vexRT[Btn5U]) {
if(abs(flyTarget - velAvg) > flyThresh + 50 || abs(lVelAvg - rVelAvg) > lRFlyThresh) {
setLeds(0);
}
else if(abs(flyTarget - velAvg) < flyThresh + 50 && abs(flyTarget - velAvg) > flyThresh) {
setLeds(1);
}
else {
setLeds(2);
}
if(abs(lVelAvg - rVelAvg) > lRFlyThresh) {
if(lVelAvg > rVelAvg) {
motor[blFly] = motor[tlFly] = 95;
motor[brFly] = motor[trFly] = 127;
}
else {
motor[brFly] = motor[trFly] = 95;
motor[blFly] = motor[tlFly] = 127;
}
}
else if(flyTarget - velAvg > flyThresh) {
setFlywheel(127);
}
else if(velAvg - flyTarget > flyThresh) {
setFlywheel(63);
}
else {
setFlywheel(79);
}
}
else if(vexRT[Btn5D] && velAvg <= 50) {
setFlywheel(-31);
}
else {
setFlywheel(0);
setLeds(3);
}
button5U2Last = vexRT[Btn5UXmtr2];
button5D2Last = vexRT[Btn5DXmtr2];
wait1Msec(20);
}
}
void Rotate90(int value){
motor[motorB] = 50 * value;
motor[motorC] = -50 * value;
wait1Msec(425);
setMotor(0);
}
VESPERSSpatialLineScanConfiguration::VESPERSSpatialLineScanConfiguration(QObject *parent)
: AMStepScanConfiguration(parent), VESPERSScanConfiguration()
{
setName("Line Scan");
setUserScanName("Line Scan");
dbObject_->setParent(this);
setIncomingChoice(VESPERS::Imini);
setFluorescenceDetector(VESPERS::SingleElement);
setMotor(VESPERS::H);
setCCDDetector(VESPERS::NoCCD);
setCCDFileName("");
setExportSpectraSources(true);
setExportSpectraInRows(true);
setOtherPosition(-123456789.0);
setCloseFastShutter(true);
setReturnToOriginalPosition(false);
AMScanAxisRegion *region = new AMScanAxisRegion;
AMScanAxis *axis = new AMScanAxis(AMScanAxis::StepAxis, region);
appendScanAxis(axis);
connect(this, SIGNAL(startChanged(double)), this, SLOT(computeTotalTime()));
connect(this, SIGNAL(stepChanged(double)), this, SLOT(computeTotalTime()));
connect(this, SIGNAL(endChanged(double)), this, SLOT(computeTotalTime()));
connect(this, SIGNAL(timeChanged(double)), this, SLOT(computeTotalTime()));
connect(dbObject_, SIGNAL(ccdDetectorChanged(int)), this, SLOT(computeTotalTime()));
}
void
Datasette::pressStop()
{
debug("Datasette::pressStop\n");
setMotor(false);
playKey = false;
}
VESPERS2DScanConfiguration::VESPERS2DScanConfiguration(QObject *parent)
: AMStepScanConfiguration(parent), VESPERSScanConfiguration()
{
setName("2D Map");
setUserScanName("2D Map");
dbObject_->setParent(this);
setIncomingChoice(VESPERS::Imini);
setFluorescenceDetector(VESPERS::SingleElement);
setMotor(VESPERS::Motors(VESPERS::H | VESPERS::V));
setCCDDetector(VESPERS::NoCCD);
setCCDFileName("");
setExportAsAscii(true);
setExportSpectraSources(true);
setExportSpectraInRows(true);
setCloseFastShutter(true);
setReturnToOriginalPosition(false);
AMScanAxisRegion *region = new AMScanAxisRegion;
AMScanAxis *axis = new AMScanAxis(AMScanAxis::StepAxis, region);
appendScanAxis(axis);
region = new AMScanAxisRegion;
axis = new AMScanAxis(AMScanAxis::StepAxis, region);
appendScanAxis(axis);
connect(scanAxisAt(0)->regionAt(0), SIGNAL(regionStartChanged(AMNumber)), this, SLOT(computeTotalTime()));
connect(scanAxisAt(0)->regionAt(0), SIGNAL(regionStepChanged(AMNumber)), this, SLOT(computeTotalTime()));
connect(scanAxisAt(0)->regionAt(0), SIGNAL(regionEndChanged(AMNumber)), this, SLOT(computeTotalTime()));
connect(scanAxisAt(0)->regionAt(0), SIGNAL(regionTimeChanged(AMNumber)), this, SLOT(computeTotalTime()));
connect(scanAxisAt(1)->regionAt(0), SIGNAL(regionStartChanged(AMNumber)), this, SLOT(computeTotalTime()));
connect(scanAxisAt(1)->regionAt(0), SIGNAL(regionStepChanged(AMNumber)), this, SLOT(computeTotalTime()));
connect(scanAxisAt(1)->regionAt(0), SIGNAL(regionEndChanged(AMNumber)), this, SLOT(computeTotalTime()));
connect(dbObject_, SIGNAL(ccdDetectorChanged(int)), this, SLOT(computeTotalTime()));
}
void SearchSaturn()
{
// Rotate 90 degrees, then start sweeping the sonar sensor
Rotate90(-1);
setMotor(0);
// Rotate in the other direction on spot
motor[motorB] = 20;
motor[motorC] = -20;
while(SensorValue[sonarSensor] > 70){
// Keep rotating
}
setMotor(0);
}
