int findBrightestLight(){
int minLightSensor = 500;
int potValue = 0;
while(SensorValue(pot) > 500){
motor(sensorMotor) = 15;
//writeDebugStreamLine("Counter Clockwise %d - %d",SensorValue(pot),SensorValue(lightSensor));
wait1Msec(100);
}
stopMotor(sensorMotor);
while(SensorValue(pot) < 3500){
motor(sensorMotor) = -13;
//writeDebugStreamLine("Clockwise %d - %d",SensorValue(pot),SensorValue(lightSensor));
if(SensorValue(lightSensor) < minLightSensor){
minLightSensor = SensorValue(lightSensor);
potValue = SensorValue(pot);
}
wait1Msec(100);
}
//writeDebugStreamLine("Stopped");
//writeDebugStreamLine("Lowest value %d at potentiometer %d", minLightSensor, potValue);
stopMotor(sensorMotor);
potValue = ((potValue-573.0) * (180.0/2698.0)) - 10;
return potValue;
}
void stopDriveMotors()
{
stopMotor(RightDriveA);
stopMotor(RightDriveB);
stopMotor(LeftDriveA);
stopMotor(LeftDriveB);
}
void turnXDegrees(int degrees){
float desiredSensor = 0;
SensorValue(leftEncoder) = 0;
SensorValue(rightEncoder) = 0;
if(degrees > 0 && degrees <=180){
desiredSensor = degrees * (279.0/180.0);
while(SensorValue(rightEncoder) < desiredSensor && SensorValue(leftEncoder) < desiredSensor){
motor(leftMotor) = 50;
motor(rightMotor) = -50;
wait1Msec(50);
}
} else if (degrees > 180 && degrees <=360) {
degrees = abs(360 - degrees);
desiredSensor = degrees * (279.0/180.0);
while(SensorValue(rightEncoder) < desiredSensor && SensorValue(leftEncoder) < desiredSensor){
motor(leftMotor) = -50;
motor(rightMotor) = 50;
wait1Msec(50);
}
}
//writeDebugStreamLine("Rotating: %d", desiredSensor);
stopMotor(leftMotor);
stopMotor(rightMotor);
}
void initMotors() {
/* Set the motor driver diagnostic pins to inputs */
leftF1::SetDirRead();
leftF2::SetDirRead();
rightF1::SetDirRead();
rightF2::SetDirRead();
/* Setup motor pins to output */
leftPWM::SetDirWrite();
leftDir::SetDirWrite();
rightPWM::SetDirWrite();
rightDir::SetDirWrite();
/* Set PWM frequency to 20kHz - see the datasheet http://www.atmel.com/Images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet_Complete.pdf page 128-138 */
// Set up PWM, Phase and Frequency Correct on pin 9 (OC1A) & pin 10 (OC1B) with ICR1 as TOP using Timer1
TCCR1B = (1 << WGM13) | (1 << CS10); // Set PWM Phase and Frequency Correct with ICR1 as TOP and no prescaling
ICR1 = PWMVALUE; // ICR1 is the TOP value - this is set so the frequency is equal to 20kHz
/* Enable PWM on pin 9 (OC1A) & pin 10 (OC1B) */
// Clear OC1A/OC1B on compare match when up-counting
// Set OC1A/OC1B on compare match when down-counting
TCCR1A = (1 << COM1A1) | (1 << COM1B1);
stopMotor(left);
stopMotor(right);
}
task main()
{ //Program begins, insert code within curly braces
startMotor(rightMotor, (127/2)); //Turns on right motor at 50% power
startMotor(leftMotor, (127/2)); //Turns on left motor at 50% power
wait(5); //Runs above code for 5 seconds
stopMotor(rightMotor); //Turns off right motor
stopMotor(leftMotor); //Turns off left motor
}
task main()
{ //Program begins, insert code within curly braces
untilSonarLessThan(20, sonar); //waits for the sonar sensor todetect an object within 20cm
startMotor(leftMotor, 63); //turns on left motor
startMotor(rightMotor, 63); //turns on right motor
untilSonarGreaterThan(25, sonar); //waits for the object to move to a distance greater than 25cm
stopMotor(leftMotor); //Stops left motor
stopMotor(rightMotor); //Stops right motor
}
void fullStop() {
digitalWrite(STANDBY, LOW);
digitalWrite(STATUS_LED, LOW); // indicate status
stopMotor(MOTA);
stopMotor(MOTB);
myservo.write(0);
SoftwareServo::refresh();
status = CMD_STOP;
}
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 manualControl(void){ //For manual control
if (abs(vexRT(Ch2)) > 15 || abs(vexRT(Ch3)) > 15){ //Only move if we pop over 15 units on the controls
//Run the motors at the joystick input
setLeftMotors(vexRT(Ch2));
setRightMotors(vexRT(Ch3));
} else {
//Kill the motors
stopLeftMotors();
stopRightMotors();
}
if (vexRT(Btn8U)){
setMotor(vertBelt,- 0);
}
// Starts and stops verticle belt
if (vexRT(Btn5D)||vexRT(Btn6D)){
if (vexRT(Btn5D)){setMotor(vertBelt,-100);} else {setMotor(vertBelt,100);}
}
// Starts and stops verticle belt
if (vexRT(Btn5U)||vexRT(Btn6U)){
if (vexRT(Btn5U)){setMotor(hozBelt,-100);} else {setMotor(hozBelt,100);}
}
//Kill the belts
if (vexRT(Btn8D)){stopMotor(hozBelt);stopMotor(vertBelt);}
//Start Auto
if (vexRT(Btn8L) && vexRT(Btn8R)){roboControl();}
//Quick start, all on
if (vexRT(Btn7L)){
setMotor(hozBelt,100);
setMotor(vertBelt,100);
launcherSpeed_new = 127;
}
/* Launcher speeds */
if (vexRT(Btn7D)){ stopLauncher();} //Emergency launcher Stop
//if (vexRT(Btn7D)){ safeLauncherStop();} //Safe launcher Stop
if (vexRT(Btn7R)){launcherSpeed_new = 75;}//Low speed mode
if (vexRT(Btn7U)){ launcherSpeed_new = 127;} //Sets launcher speed to 127 (max)
updateLauncherSpeed();
}
task main()
{ //Program begins, insert code within curly braces
while (true)
{
untilBump(bumpSwitch); //after bump switch is pressed and released
startMotor(rightMotor, 127/2);
startMotor(leftMotor, 127/2);
untilBump(bumpSwitch);
startMotor(rightMotor, -127/2);
startMotor(leftMotor, -127/2);
wait(3.5);
stopMotor(rightMotor);
stopMotor(leftMotor);
}
}
void manualControl(void){ //For manual control
if (abs(vexRT(Ch2)) > 15 || abs(vexRT(Ch3)) > 15){ //Only move if we pop over 15 units on the controls
//Run the motors at the joystick input
setLeftMotors(vexRT(Ch2));
setRightMotors(vexRT(Ch3));
} else {
//Kill the motors
stopLeftMotors();
stopRightMotors();
}
// Starts and stops verticle belt
if (vexRT(Btn5D)||vexRT(Btn6D)){
if (vexRT(Btn5D)){setMotor(vertBelt,-100);} else {setMotor(vertBelt,100);}
}
// Starts and stops verticle belt
if (vexRT(Btn5U)||vexRT(Btn6U)){
if (vexRT(Btn5U)){setMotor(hozBelt,-100);} else {setMotor(hozBelt,100);}
}
//Kill the belts
if (vexRT(Btn8D)){stopMotor(hozBelt);stopMotor(vertBelt);}
// Starts and stops the horizontal belt
/*
if (vexRT(Btn8L)){hozBeltBtn_beenPressed = true;} //We have pressed it
if (!vexRT(Btn8L) && hozBeltBtn_beenPressed){ //When we let go lets flip
if (!hozBelt_on){hozBelt_on = true; setMotor(hozBelt,100);}else{hozBelt_on = false;stopMotor(hozBelt);}
hozBeltBtn_beenPressed = false;
}
*/
//Quick start, all on
if (vexRT(Btn7L)){
setMotor(hozBelt,100);
setMotor(vertBelt,100);
launcherSpeed_new = 127;
}
/* Launcher speeds */
if (vexRT(Btn7D)){ stopLauncher();} //Emergency launcher Stop
//if (vexRT(Btn7D)){ safeLauncherStop();} //Safe launcher Stop
if (vexRT(Btn7R)){launcherSpeedHop();}//Pop up 15
if (vexRT(Btn7U)){ launcherSpeed_new = 127;} //Sets launcher speed to 127 (max)
updateLauncherSpeed();
}
void checkMotorTimer(byte i)
{
if (time1[motorCtrl[i].timer] >= motorCtrl[i].ms) {
stopMotor(motorCtrl[i].motorPort1);
if (motorCtrl[i].motorPort1 != motorCtrl[i].motorPort2) stopMotor(motorCtrl[i].motorPort2);
//writeDebugStreamLine("Motor timer stop");
// Clear array
motorCtrl[i].ms = 0;
motorCtrl[i].powerPct = 0;
motorCtrl[i].slowPct = 0;
motorCtrl[i].brake = false;
motorCtrl[i].encoderInUse = false;
motorCtrl[i].active = false;
}
}
int stop(TCan *can)
{
stopMotor(can);
setUnitMode(can, MODE_POS); /* UnitMode must be MODE_POS for ST to work. */
unsigned char data[8] = { 0x53, 0x54, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; /* ST */
return sendPDO2DiscardReply(can, 4, data);
}
void L293d::setDirection(bool direction) {
this->direction = direction;
if (running) {
stopMotor();
startMotor();
}
}
void L293d::changeDirection() {
direction = !direction;
if (running) {
stopMotor();
startMotor();
}
}
void setupMotor(Motor motor) {
pinMode(motor.pin_1, OUTPUT);
pinMode(motor.pin_2, OUTPUT);
stopMotor(motor);
softPwmCreate(motor.pin_speed, 0, 100);
setSpeedMotor(motor, 0);
}
void JankyEncoder::Run(void)
{
if (pEncoder)
{
//printf ("GoodEncoder\n");
SmartDashboard::PutNumber ("Encodercount", pEncoder->Get());
if (bEncoding == true)
{
if (pEncoder->Get() >= targetcount || maxTimer->Get() >= desiredMaxTime)
{
bDone = true;
//printf ("Task targetcount reached\n");
stopMotor();
maxTimer->Stop();
}
else
{
printf ("Targetcount NOT reached\n");
startMotor();
}
}
}
else
{
printf ("WARNING: TASK: pEncoder is NULL\n");
bEncoding = false;
}
}
void checkMotorPosition(byte i)
{
if (!motorCtrl[i].encoderInUse) return;
// Get difference between current position and encoder stopping position
int posDiff = abs(SensorValue[motorCtrl[i].encoderPort] - motorCtrl[i].encoderStopPos);
int lastPosDiff = abs(motorCtrl[i].lastKnownPos - motorCtrl[i].encoderStopPos);
// Set new slower motor speed when getting close to final position
if (posDiff < abs(motorCtrl[i].encoderStopPos - motorCtrl[i].encoderSlowPos)) {
turnMotor(motorCtrl[i].motorPort1, motorCtrl[i].motorPort2, motorCtrl[i].slowPct);
}
// Stop motor and timer when encoder position is reached or overshot (or turning the wrong way)
if (posDiff < 50 && posDiff - 1 > lastPosDiff) { // subtract a bit to allow for some sensor error
//writeDebugStreamLine("posDiff: %d, lastPosDiff: %d", posDiff, lastPosDiff);
// Slight motor reverse acts as brake
if (motorCtrl[i].brake) {
turnMotor(motorCtrl[i].motorPort1, motorCtrl[i].motorPort2, (motorCtrl[i].powerPct < 0 ? 20 : -20));
waitInMilliseconds(40);
}
// Experiment with fixing overshot by keeping motor reverse running a little longer
if (abs(SensorValue[motorCtrl[i].encoderPort] - motorCtrl[i].encoderStopPos) > 40) {
//writeDebugStreamLine("Overshot adj, reqt pos: %d, cur pos: %d",
// motorCtrl[i].encoderStopPos, SensorValue[motorCtrl[i].encoderPort]);
waitInMilliseconds(30);
}
// Stop Motor
stopMotor(motorCtrl[i].motorPort1);
if (motorCtrl[i].motorPort1 != motorCtrl[i].motorPort2) stopMotor(motorCtrl[i].motorPort2);
//writeDebugStreamLine("Encoder stop, reqt pos: %d, cur pos: %d",
// motorCtrl[i].encoderStopPos, SensorValue[motorCtrl[i].encoderPort]);
// Clear array
motorCtrl[i].ms = 0;
motorCtrl[i].powerPct = 0;
motorCtrl[i].slowPct = 0;
motorCtrl[i].brake = false;
motorCtrl[i].encoderInUse = false;
motorCtrl[i].active = false;
}
motorCtrl[i].lastKnownPos = SensorValue[motorCtrl[i].encoderPort];
}
/*
Remote:
Ch 3 = left stick, up and down
Ch 2 = right stick, up and down
Btn 8 = right set of buttons
Btn 5 = left triggers/bumper buttons
*/
void goForwardInches(int inches, bool stopIfWall){
// 580 sensor = 12.5 inches forward
float desiredSensor = inches * (580.0/12.5);
SensorValue(leftEncoder) = 0;
SensorValue(rightEncoder) = 0;
//writeDebugStreamLine("Moving: %d inches", inches);
while(SensorValue(rightEncoder) < desiredSensor && SensorValue(leftEncoder) < desiredSensor){
if(SensorValue(distance) >= 0 && SensorValue(distance) <= 6) {
break;
}
motor(leftMotor) = 50;
motor(rightMotor) = 50;
wait1Msec(50);
}
stopMotor(leftMotor);
stopMotor(rightMotor);
}
void motor(){
if(motorStateChange!=motorState){
motorStateChange = motorState;
{
int8_t difference = currentPosition - lastPosition;
if(difference > 0 && motorState == 4)
{
currentPosition--;
}
else if (difference < 0 && motorState == 3)
{
currentPosition++;
}
}
}
if(TCNT1>SECOND/speedModifier){
TCNT1 = 0;
switch(motorState){
case 0:
stopMotor();
return;
case 1:
// motor idle...
if(!motorInitialized){
initMotor();
motorInitialized = 1;
}
return;
case 3:
/* double forward motion */
if(steps>0){
steps--;
lastPosition = currentPosition-1;
currentPosition++;
moveMotor();
} else {
motorState = 1;
}
return;
case 4:
/* double backward motion */
if(steps>0){
steps--;
lastPosition = currentPosition+1;
currentPosition--;
moveMotor();
} else {
motorState = 1;
}
return;
}
}
}
int setForce(TCan *can, float force)
{
int rval;
stopMotor(can); /* The motor must be stopped before any changes in the unit mode can be made. */
rval = setUnitMode(can, MODE_TORQUE);
rval |= startMotor(can);
rval |= setTorque(can, force);
return rval;
}
int setPosition(TCan *can, int pos)
{
int rval;
stopMotor(can); /* The motor must be stopped before any changes in the unit mode can be made. */
rval = setUnitMode(can, MODE_POS);
rval |= startMotor(can);
rval |= setAbsolutePosition(can, pos);
rval |= beginMotion(can);
return rval;
}
task main()
{ //Program begins, insert code within curly braces
int motorCount;
motorCount = 0;
while (motorCount < 3)
{
startMotor(rightMotor, 95);
wait(2);
stopMotor(rightMotor);
wait(2);
motorCount++; //increment motorCount by 1, same as motorCount += 1 or motorCount = motorCount + 1
}
}
void doMotorStuff(Motor motor, int i) {
if (i == 0) {
stopMotor(motor);
} else {
if (i > 0) {
goForwardMotor(motor);
} else {
goBackwardMotor(motor);
}
setSpeedMotor(motor, abs(i));
}
}
void JankyEncoder::Reset(void)
{
stopMotor();
maxTimer->Reset();
Wait(RESET_WAIT);
if (pEncoder)
{
pEncoder->Reset(); //Mentor Bob says there's more to do
}
bDone = false;
bEncoding = false;
//printf ("RESETTTING\n");
}
/*
* Motor-Stellung und Schalter prüfen, ggf. Motor stoppen.
*/
void checkMotor() {
// Fall: Motor "auf"
if (isMotorOpen) {
// Motor darf nur "auf" drehen, wenn der Auf-Endstop nicht erreicht ist
if (isEndstopOpen)
stopMotor();
}
// Fall: Motor "zu"
if (isMotorClose) {
// schließen nur bei geschlossener Tür
if (!isDoorClosed || isBlocked)
stopMotor();
// Motor darf nur "zu" drehen, wenn der Zu-Endstop nicht erreicht ist
if (isEndstopClose)
stopMotor();
// fertig, wenn das Schloss nicht mehr offen ist
if (!isLockOpen)
stopMotor();
}
}
void moveInches(int inches){
bool hasMoved = false;
int leftTarget=41*inches;
int rightTarget=41*inches;
SensorValue[leftEncoder]=0;
SensorValue[rightEncoder]=0;
int ctr =0;
while(hasMoved==false){
if(SensorValue[leftEncoder] <leftTarget){
setMotor(leftTread,60);
}
else
{
stopMotor(leftTread);
ctr++;
}
if(SensorValue[rightEncoder] <rightTarget){
setMotor(rightTread,50);
}
else
{
stopMotor(rightTread);
ctr++;
}
if(ctr==2){
hasMoved = true;
}
}
}
void moveToPreviousLine()
{
// if already on a black line move off it
while(SensorValue[light] <= 50){
motor[leftMotor] = -speed;
motor[rightMotor] = -speed;
wait1Msec(1);
}
while(SensorValue[light] > 50){
motor[leftMotor] = -speed;
motor[rightMotor] = -speed;
wait1Msec(1);
}
stopMotor(200);
}
void setMotor(uint8_t speed, enum direnum direction)
{
// on direction change: stop motor and wait for a few msec
if (lastdirection!=direction && speed) {
stopMotor();
lastspeed = 0;
_delay_ms(30);
}
lastdirection = direction;
if (lastspeed==speed)
return;
lastspeed = speed;
if (speed) {
if ((direction==DirectionDown && inverted_direction) || (direction==DirectionUp && !inverted_direction))
CURTAIN_MOTOR_DIRECTION_PORT |= _BV(CURTAIN_MOTOR_DIRECTION_PIN);
else
CURTAIN_MOTOR_DIRECTION_PORT &= (uint8_t)~_BV(CURTAIN_MOTOR_DIRECTION_PIN);
if (direction==DirectionUp && sensorLimitSwitchOpen)
return;
if (sensorLimitSwitchOpen && lastdirection==DirectionUp) {
stopMotor();
return;
}
resetTicksCounter();
OCR0A = speed;
startMotor();
} else
stopMotor();
}
void moveToNextLine(){
// if already on a black line move off it
while(SensorValue[light] < 40){
motor[leftMotor] = speed;
motor[rightMotor] = speed;
wait1Msec(1);
}
// now move to the next black line
while(SensorValue[light] > 40){
motor[leftMotor] = speed;
motor[rightMotor] = speed;
wait1Msec(1);
}
motor[leftMotor] = speed;
motor[rightMotor] = speed;
wait1Msec(200);
stopMotor(200);
}
