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
}
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
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 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 L293d::changeDirection() {
direction = !direction;
if (running) {
stopMotor();
startMotor();
}
}
void L293d::setDirection(bool direction) {
this->direction = direction;
if (running) {
stopMotor();
startMotor();
}
}
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;
}
int Motor::setspeed(int16_t duty) {
if(ramp) return -2;
if(hbridge_enable.readPins() == 0) {
PRINTF("motor wasnt enabled, starting\n");
startMotor();
}
if((duty < 0) && (lastDutyCycle > 0)) {
// switchDirection(duty);
PRINTF("changing direction\n");
HBRIDGE_A_INA.setPins(~HBRIDGE_A_INA.readPins());
HBRIDGE_A_INB.setPins(~HBRIDGE_A_INB.readPins());
clockwise = false;
} else if( (duty > 0 )&&(lastDutyCycle < 0)) {
PRINTF("changing direction\n");
// switchDirection(duty);
HBRIDGE_A_INA.setPins(~HBRIDGE_A_INA.readPins());
HBRIDGE_A_INB.setPins(~HBRIDGE_A_INB.readPins());
clockwise = true;
}
MotorPWM.write(abs(duty));
// rampe fahren falls duty cycle zu stark springt (-> big wheel draws too much current if sudden hcnage in duty cycle)
// if(abs((duty - lastDutyCycle)) >= MOTOR_RAMP_THRESHOLD){
// ramp = true;
// PRINTF("too sudden change in control! current %d controlVal %d\n",duty,lastDutyCycle);
// if(duty < lastDutyCycle){
// spinDownTo(lastDutyCycle,duty);
// } else if(duty > lastDutyCycle){
// spinUpTo(lastDutyCycle,duty);
// }
// }
lastDutyCycle = duty;
// if(duty != 0){
// if((duty < 0) && clockwise){
// MotorPWM.write(duty);
//
// } else if ((duty > 0) && (clockwise == false)){
//
// MotorPWM.write(duty);
// } else MotorPWM.write(duty);
// } else stopMotor();
// int k = dcdc_read.readPins();
// PRINTF("power is: %d\n",k);
// k = hbridge_enable.readPins();
// PRINTF("should be: %d\n",k);
// switchDirection(duty);
// pwm.write(abs(duty));
// dutyCycle = duty;
// MotorPWM.write(duty);
}
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
}
}
task main()
{ //Program begins, insert code within curly braces
while (true)
{
//Input Conveior Belt
startMotor(InputBeltMotor, 127);
wait(2.5);
stopMotor(InputBeltMotor);
//Elevator
startMotor(ElevatorMotor, 127);
wait(1.5);
stopMotor(ElevatorMotor);
//Move Cookie To line follower
do
{
startMotor(OutputBeltMotor, -127);
}
while(SensorValue(linefollower) > 2900);
stopMotor(OutputBeltMotor);
//Reset Elevator
startMotor(ElevatorMotor, -127);
wait(2);
stopMotor(ElevatorMotor);
//Move Cookie To Whip Cream
startMotor(OutputBeltMotor, -127);
wait(0.4);
stopMotor(OutputBeltMotor);
//Whip Cream Press
startMotor(WhipCreamMotor, -127);
wait(1);
stopMotor(WhipCreamMotor);
//Whip Cream Reset
startMotor(WhipCreamMotor, 127);
wait(0.9);
stopMotor(WhipCreamMotor);
//Output Conveior Belt
startMotor(OutputBeltMotor, -127);
wait(2);
}
}
bool JankyEncoder::Go(void)
{
printf ("INSIDE GO\n");
if (bEncoding == false)
{
printf ("bEncoding = false\n");
startMotor();
maxTimer->Start();
bEncoding = true;
return true;
}
else
{
return false;
}
}
void Motor::run() {
PRINTF("Starting Motor\n");
startMotor();
// suspendCallerUntil(END_OF_TIME);
// pwm.write(250);
// int cnt = 0;
// while(1){
// setspeed(cnt++);
//// suspendCallerUntil(NOW()+100*MILLISECONDS);
// PRINTF("speed is now %d\n",cnt);
// ORANGE_TOGGLE;
//// cnt++;
// if(cnt > 999){
// cnt = -1000;
// }
// }
}
void moveToLocation(int newPosition)
{
if(newPosition != currentPosition){
startMotor();
Delay10KTCYx(7);
while(!readIRSensor());
stopMotor();
//Track the current location of the plant
currentPosition++;
if(currentPosition == 3){
currentPosition = 0;
}
//recursive call to move the the next position if this is not the required state
if(newPosition != currentPosition){
moveToLocation(newPosition);
}
}
}
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();
}
int main(void)
{
// initialisieren
init();
// Bereit-Meldung
_delay_ms(250);
setLED(COL_RED, LED_FAST);
_delay_ms(1000);
setLED(COL_RED, LED_OFF);
setLED(COL_GREEN, LED_FAST);
_delay_ms(700);
setLED(COL_GREEN, LED_OFF);
while(1) {
checkMotor();
// bei offener Tür immer auch das Schloss öffnen!
if (!isDoorClosed && !isFullyOpen()) {
startMotor(MOTOR_OPEN);
blockMotor();
}
else if (isSetOpen) {
startMotor(MOTOR_OPEN);
}
else if (isSetClose) {
startMotor(MOTOR_CLOSE);
} else if (!isDoorClosed && !isFullyOpen()) {
// bei unklarem Status: tür auf
startMotor(MOTOR_OPEN);
}
// Es wird nur einer der Stati rot/grün angezeigt,
// wenn die Tür "geschlossen" signalisiert, wird rot bevorzugt,
// bei gleichzeitigem "offen"-status blinkt grün schnell
// (dann liegt ein unzulässiger Zustand vor).
// grün blinken, wenn Motor Richtung "auf"
if (isMotorOpen)
setLED(COL_GREEN, LED_SLOW);
// grün, wenn komplett offen, sonst aus
else if (isFullyOpen())
setLED(COL_GREEN, isFullyClosed() ? LED_FAST : LED_ON);
// setLED(COL_GREEN, isFullyClosed() ? LED_OFF : LED_ON);
else
setLED(COL_GREEN, LED_OFF);
// rot blinken, wenn Motor Richtung "zu"
if (isMotorClose)
setLED(COL_RED, LED_SLOW);
// rot, wenn komplett geschlossen, sonst aus
else
setLED(COL_RED, isFullyClosed() ? LED_ON : LED_OFF);
} // while
return 0;
}
