task main() { // Initializing system state variables float motorAngleRaw, // The angle of the "motor", measured in degrees. We will take the average of both motor positions, wich is essentially how far the middle of the robot has traveled. motorAngle, // The angle of the motor, converted to radians (2*pi radians equals 360 degrees). motorAngleReference = 0, // The reference angle of the motor. The robot will attempt to drive forward or backward, such that its measured position equals this reference (or close enough). motorAngleError, // The error: the deviation of the measured motor angle from the reference. The robot attempts to make this zero, by driving toward the reference. motorAngleErrorAccumulated = 0, // We add up all of the motor angle error in time. If this value gets out of hand, we can use it to drive the robot back to the reference position a bit quicker. motorAngularSpeed, // The motor speed, estimated by how far the motor has turned in a given amount of time motorAngularSpeedReference = 0, // The reference speed during manouvers: how fast we would like to drive, measured in radians per second. motorAngularSpeedError, // The error: the deviation of the motor speed from the reference speed. motorDutyCycle, // The 'voltage' signal we send to the motor. We calulate a new value each time, just right to keep the robot upright. gyroRateRaw, // The raw value from the gyro sensor in rate mode. gyroRate, // The angular rate of the robot (how fast it is falling forward), measured in radians per second. gyroEstimatedAngle = 0, // The gyro doesn't measure the angle of the robot, but we can estimate this angle by keeping track of the gyroRate value in time. gyroOffset = 0; // Over time, the gyro rate value can drift. This causes the sensor to think it is moving even when it is perfectly still. We keep track of this offset. // Set the LED to blue while calibrating the gyro setTouchLEDRGB(touchLed, 0, 0, 255); //Set the motor brake mode setMotorBrakeMode(right,motorBrake); setMotorBrakeMode(left,motorBrake); //Calculating the (initial) avrage gyro sensor value const int gyroRateCalibrateCount = 100; for (int i = 0; i < gyroRateCalibrateCount; i++){ gyroOffset = gyroOffset + getGyroRate(gyro); sleep(10); } //The offset is equal to the long term average value of the sensor. gyroOffset = gyroOffset/gyroRateCalibrateCount; //Timing settings for the program const int loopTimeMiliSec = 20, // Time of each loop, measured in miliseconds. motorAngleHistoryLength = 4; // Number of previous motor angles we keep track of. const float loopTimeSec = loopTimeMiliSec/1000.0, // Time of each loop, measured in seconds. radiansPerDegree = PI/180.0, // The number of radians in a degree. radiansPerSecondPerRawGyroUnit = radiansPerDegree, // For the VEX IQ, 1 unit = 1 deg/s radiansPerRawMotorUnit = radiansPerDegree, // For the VEX IQ, 1 unit = 1 deg gyroDriftCompensationRate = 0.1*loopTimeSec, // The rate at which we'll update the gyro offset degPerSecPerPercentSpeed = 7.1, // On the VEX IQ, "1% speed" corresponds to 7.1 deg/s (if speed control were enabled) radPerSecPerPercentSpeed = degPerSecPerPercentSpeed * radiansPerDegree; //Convert this number to the speed in rad/s per "percent speed" //A (fifo) array which we'll use to keep track of previous motor positions, which we can use to calculate the rate of change (speed) float motorAngleHistory[motorAngleHistoryLength]; memset(motorAngleHistory,0,4*motorAngleHistoryLength); int motorAngleIndex = 0; const float gainGyroAngle = 900, //For every radian (57 degrees) we lean forward, apply this amount of duty cycle. gainGyroRate = 36 , //For every radian/s we fall forward, apply this amount of duty cycle. gainMotorAngle = 15, //For every radian we are ahead of the reference, apply this amount of duty cycle gainMotorAngularSpeed = 9.6, //For every radian/s drive faster than the reference value, apply this amount of duty cycle gainMotorAngleErrorAccumulated = 3; //For every radian x s of accumulated motor angle, apply this amount of duty cycle // Variables to control the reference speed and steering float speed = 0, steering = 0; //Joystick positions int joyStickLeft = 0, joyStickRight = 0; //Maximum speed and steering when remote joysticks are fully moved forward const int kMaxRemoteSpeed = 20; const int kMaxRemoteSteering = 10; //Initialization complete setTouchLEDRGB(touchLed, 0, 255, 0); //Run the main loop until the touch LED is touched. while(getTouchLEDValue(touchLed)==0) { /////////////////////////////////////////////////////////////// // // Driving and Steering. Modify the <<speed>> and <<steering>> // variables as you like to make your segway go anywhere! // // (If you don't have the controller, just delete the four lines // below. Then <<speed>> and <<steering>> remain zero.) // /////////////////////////////////////////////////////////////// joyStickLeft = getJoystickValue(ChA); joyStickRight = getJoystickValue(ChD); speed = (joyStickRight + joyStickLeft)/2.0 * (kMaxRemoteSpeed /100.0); steering = (joyStickRight - joyStickLeft)/2.0 * (kMaxRemoteSteering/100.0); /////////////////////////////////////////////////////////////// // (You don't need to modify anything in the sections below.) /////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////// // Reading the Gyro. /////////////////////////////////////////////////////////////// gyroRateRaw = getGyroRateFloat(gyro); gyroRate = (gyroRateRaw - gyroOffset)*radiansPerSecondPerRawGyroUnit; /////////////////////////////////////////////////////////////// // Reading the Motor Position /////////////////////////////////////////////////////////////// motorAngleRaw = (getMotorEncoder(right) + getMotorEncoder(left))/2.0; motorAngle = motorAngleRaw*radiansPerRawMotorUnit; motorAngularSpeedReference = speed*radPerSecPerPercentSpeed; motorAngleReference = motorAngleReference + motorAngularSpeedReference*loopTimeSec; motorAngleError = motorAngle - motorAngleReference; /////////////////////////////////////////////////////////////// // Computing Motor Speed /////////////////////////////////////////////////////////////// motorAngularSpeed = (motorAngle - motorAngleHistory[motorAngleIndex])/(motorAngleHistoryLength*loopTimeSec); motorAngleHistory[motorAngleIndex] = motorAngle; motorAngularSpeedError = motorAngularSpeed; /////////////////////////////////////////////////////////////// // Computing the motor duty cycle value /////////////////////////////////////////////////////////////// motorDutyCycle = gainGyroAngle * gyroEstimatedAngle + gainGyroRate * gyroRate + gainMotorAngle * motorAngleError + gainMotorAngularSpeed * motorAngularSpeedError + gainMotorAngleErrorAccumulated * motorAngleErrorAccumulated; /////////////////////////////////////////////////////////////// // Apply the signal to the motor, and add steering /////////////////////////////////////////////////////////////// setMotorSpeed(right, motorDutyCycle + steering); setMotorSpeed(left, motorDutyCycle - steering); /////////////////////////////////////////////////////////////// // Update angle estimate and Gyro Offset Estimate /////////////////////////////////////////////////////////////// gyroEstimatedAngle = gyroEstimatedAngle + gyroRate*loopTimeSec; gyroOffset = (1-gyroDriftCompensationRate)*gyroOffset+gyroDriftCompensationRate*gyroRateRaw; /////////////////////////////////////////////////////////////// // Update Accumulated Motor Error /////////////////////////////////////////////////////////////// motorAngleErrorAccumulated = motorAngleErrorAccumulated + motorAngleError*loopTimeSec; motorAngleIndex = (motorAngleIndex + 1) % motorAngleHistoryLength; /////////////////////////////////////////////////////////////// // Wait for the loop to complete /////////////////////////////////////////////////////////////// sleep(loopTimeMiliSec); } /////////////////////////////////////////////////////////////// // Turn off the motors, and end the program. /////////////////////////////////////////////////////////////// setTouchLEDRGB(touchLed, 255, 0, 0); setMotorSpeed(left,0); setMotorSpeed(right,0); }
void pivotRight(int speed){ resetMotorEncoder(motorB); resetMotorEncoder(motorC); setMotorSpeed(motorB, speed); setMotorSpeed(motorC, -speed); }
int main() { // Handle Ctrl-C quit signal(SIGINT, sig_handler); //Motor Stuff mraa::Pwm pwm = mraa::Pwm(9); pwm.write(0.0); pwm.enable(true); //assert(pwm != NULL); mraa::Gpio dir = mraa::Gpio(8); //assert(dir != NULL); dir.dir(mraa::DIR_OUT); dir.write(0); mraa::Pwm pwm2 = mraa::Pwm(6); pwm2.write(0.0); pwm2.enable(true); //assert(pwm2 != NULL); mraa::Gpio dir2 = mraa::Gpio(5); //assert(dir != NULL); dir2.dir(mraa::DIR_OUT); dir2.write(0); mraa::Aio aioBackInfrared = mraa::Aio(BACK_INFRARED_PIN); mraa::Aio aioFrontInfrared = mraa::Aio(FRONT_INFRARED_PIN); mraa::Aio aioHeadInfrared = mraa::Aio(HEAD_INFRARED_PIN); float speedWallFollower = .1; float powerWallFollower = 0; float forwardBiasWallFollower = .15; float backDistance = 0; float frontDistance = 0; float headDistance = 10.0; float desiredDistance = 5; float P_CONSTANT_WALL_FOLLOWER = .15; while (running) { float backInfraredReading = aioBackInfrared.read(); float frontInfraredReading = aioFrontInfrared.read(); float headInfraredReading = aioHeadInfrared.read(); printf("Infra readings: back: %f, front: %f, head: %f\n", backInfraredReading, frontInfraredReading, headInfraredReading); backDistance = (backDistance * alpha_infrareds) + (infraReadingToDistanceBack(backInfraredReading) * (1.0 - alpha_infrareds)); frontDistance = (frontDistance * alpha_infrareds) + (infraReadingToDistanceFront(frontInfraredReading) * (1.0 - alpha_infrareds) * .94); headDistance = (headDistance * alpha_infrareds) + (infraReadingToDistanceHead(headInfraredReading) * (1.0 - alpha_infrareds)); printf("Distances: Back: %f, Front: %f, Head: %f\n", backDistance, frontDistance, headDistance); float averageDistance = (backDistance + frontDistance) / 2.0; float diffDistance = desiredDistance - averageDistance; powerWallFollower = speedWallFollower * (P_CONSTANT_WALL_FOLLOWER * diffDistance); if (powerWallFollower > .3) { powerWallFollower = .3; } else if (powerWallFollower < -.3) { powerWallFollower = -.3; } if (headDistance < 6.0 && headDistance > 0){ printf("PowerWallFollower reduced!\n"); //head hit a wall powerWallFollower = .2; setMotorSpeed(pwm, dir, powerWallFollower); setMotorSpeed(pwm2, dir2, powerWallFollower); usleep(1000 * 20); } else if ((backDistance > 20) && (frontDistance < 20 && frontDistance > 0 )){ powerWallFollower = .15; //only front distance gives good readings, turn left setMotorSpeed(pwm, dir, powerWallFollower + forwardBiasWallFollower); setMotorSpeed(pwm2, dir2, powerWallFollower - forwardBiasWallFollower); } else if ((frontDistance > 20) && (backDistance < 20 && backDistance > 0 )){ setMotorSpeed(pwm, dir, forwardBiasWallFollower); setMotorSpeed(pwm2, dir2, -1.0 * forwardBiasWallFollower); usleep(300 * 1000); powerWallFollower = -.15; //only back distance gives good readings, turn right setMotorSpeed(pwm, dir, powerWallFollower + forwardBiasWallFollower); setMotorSpeed(pwm2, dir2, powerWallFollower - forwardBiasWallFollower); usleep(300 * 1000); } else if ((frontDistance > 20 || frontDistance < 0) && (backDistance > 20 || backDistance < 0)) { powerWallFollower = .15; //sensors read garbage setMotorSpeed(pwm, dir, powerWallFollower); setMotorSpeed(pwm2, dir2, -1 * powerWallFollower); } else if ((frontDistance < 20 && frontDistance > 0) && (backDistance < 20 && backDistance > 0)) { setMotorSpeed(pwm, dir, powerWallFollower + forwardBiasWallFollower); //normal behavior setMotorSpeed(pwm2, dir2, powerWallFollower - forwardBiasWallFollower); } else { setMotorSpeed(pwm, dir, 0); setMotorSpeed(pwm2, dir2, 0); running = 0; } printf("Set power to: %f\n", powerWallFollower); } setMotorSpeed(pwm, dir, 0); setMotorSpeed(pwm2, dir2, 0); return 0; }
void Controllingroomba::on_pbDriveBackward_released() { emit setMotorSpeed(0,0); }
void Controllingroomba::on_pbDriveRight_released() { emit setMotorSpeed(0,0); }
task main() { int leftMotorSpeed, rightMotorSpeed, wristMotorSpeed, armMotorSpeed, stickAValue, stickBValue, stickCValue, stickDValue; resetMotorEncoder(armMotor); resetMotorEncoder(clawMotor); resetMotorEncoder(leftMotor); resetMotorEncoder(rightMotor); resetMotorEncoder(wristMotor); resetGyro(gyro); startGyroCalibration(gyro, gyroCalibrateSamples2048); eraseDisplay(); getGyroCalibrationFlag(gyro); displayString(3, "calibrating gyro"); wait1Msec(500); eraseDisplay(); startTask(displayMyMotorPositions); while (true ){ /***************************************************************************************************************/ //Joystick Control: /***************************************************************************************************************/ //Driving: if(getJoystickValue(BtnEUp) > 0) { //Drive Straight Forward driveStraightForward(); } else if(getJoystickValue(BtnEDown) > 0) { //Drive Straight Backward driveStraightBackward(); } else { driveForwardPressed = false; driveBackwardPressed = false; stickAValue = getJoystickValue(ChA); if ((stickAValue <= 15) && (stickAValue >= -15) ) stickAValue = 0; stickBValue = getJoystickValue(ChB); if ((stickBValue <= 15) && (stickBValue >= -15) ) stickBValue = 0; if (stickBValue >=0 ) stickBValue = (stickBValue / 10) * (stickBValue / 10) * 2; else stickBValue = - (stickBValue / 10) * (stickBValue / 10) * 2; leftMotorSpeed = stickAValue - stickBValue; rightMotorSpeed = stickAValue + stickBValue; if ( leftMotorSpeed > 100) leftMotorSpeed = 100; if ( leftMotorSpeed < -100) leftMotorSpeed = -100; if ( rightMotorSpeed > 100) rightMotorSpeed = 100; if ( rightMotorSpeed < -100) rightMotorSpeed = -100; if (leftMotorSpeed >=0 ) leftMotorSpeed = (leftMotorSpeed / 10) * (leftMotorSpeed / 10); else leftMotorSpeed = - (leftMotorSpeed / 10) * (leftMotorSpeed / 10); if (rightMotorSpeed >=0 ) rightMotorSpeed = (rightMotorSpeed / 10) * (rightMotorSpeed / 10); else rightMotorSpeed = - (rightMotorSpeed / 10) * (rightMotorSpeed / 10); setMotorSpeed(leftMotor, leftMotorSpeed); setMotorSpeed(rightMotor, rightMotorSpeed); } /***************************************************************************************************************/ //WRIST MOTOR stickDValue = getJoystickValue(ChD); if ((stickDValue <= 15) && (stickDValue >= -15) ) stickDValue = 0; wristMotorSpeed = stickDValue; if (wristMotorSpeed >=0 ) wristMotorSpeed = (wristMotorSpeed / 10) * (wristMotorSpeed / 10); else wristMotorSpeed = - (wristMotorSpeed / 10) * (wristMotorSpeed / 10); globalWristPosition = getMotorEncoder(wristMotor); if ((wristMotorSpeed > 0) && (globalWristPosition >= WRIST_UPPER_LIMIT)){ setMotorTarget(wristMotor, WRIST_UPPER_LIMIT, 70); } else if ((wristMotorSpeed < 0) && (globalWristPosition <= WRIST_LOWER_LIMIT)) { setMotorTarget(wristMotor, WRIST_LOWER_LIMIT, 70); } else { //slow things down if we are near the limit of wrist movement if ( abs(globalWristPosition - WRIST_LOWER_LIMIT) < 10) { wristMotorSpeed = wristMotorSpeed / 4; } if ( abs(globalWristPosition - WRIST_UPPER_LIMIT) < 10) { wristMotorSpeed = wristMotorSpeed / 4; } setMotorSpeed(wristMotor, wristMotorSpeed ); } /***************************************************************************************************************/ //ARM MOTOR stickCValue = getJoystickValue(ChC); if ((stickCValue <= 15) && (stickCValue >= -15) ) stickCValue = 0; armMotorSpeed = stickCValue; if (armMotorSpeed >=0 ) armMotorSpeed = (armMotorSpeed / 10) * (armMotorSpeed / 10); else armMotorSpeed = - (armMotorSpeed / 10) * (armMotorSpeed / 10); globalArmPosition = getMotorEncoder(armMotor); if ((armMotorSpeed > 0) && (globalArmPosition >= ARM_UPPER_LIMIT)){ setMotorTarget(armMotor, ARM_UPPER_LIMIT, 70); } else if ((armMotorSpeed < 0) && (globalArmPosition <= ARM_LOWER_LIMIT)) { setMotorTarget(armMotor, ARM_LOWER_LIMIT, 70); } else { //slow things down if we are near the limit of arm movement if ( abs(globalArmPosition - ARM_LOWER_LIMIT) < 10){ armMotorSpeed= armMotorSpeed/ 4; } if ( abs(globalArmPosition - ARM_UPPER_LIMIT) < 10){ armMotorSpeed= armMotorSpeed/ 4; } setMotorSpeed(armMotor, armMotorSpeed); } /***************************************************************************************************************/ //CLAW MOTOR if(getJoystickValue(BtnLUp) > 0) { //CLOSE setMotorSpeed(clawMotor, 70); } else if(getJoystickValue(BtnLDown) > 0) { //OPEN globalClawPosition = getMotorEncoder(clawMotor); if (globalClawPosition <= -87) { setMotorTarget(clawMotor, -87, 70); } else { setMotorSpeed(clawMotor, -70); } } else { globalClawPosition = getMotorEncoder(clawMotor); setMotorTarget(clawMotor, globalClawPosition, 90); } /***************************************************************************************************************/ //Buttons to move our Arm for Us quickly to other positions if(getJoystickValue(BtnFUp) > 0) { //UP moveToAimingPosition(); } if(getJoystickValue(BtnFDown) > 0) { //GRAB a Block moveToNeutralPosition(); } /***************************************************************************************************************/ //Block Stacking Positions //Buttons to move our Arm for Us quickly to other positions if(getJoystickValue(BtnRUp) > 0) { //Upper Stacking Position moveToUpperStackingPosition(); } if(getJoystickValue(BtnRDown) > 0) { //Lower Stacking Position moveToLowerStackingPosition(); } wait1Msec(50); // Give actions time to make progress and prevent over-control from taking inputs too fast } }
void Controllingroomba::on_pbDriveLeft_pressed() { emit setMotorSpeed((ui->hsMotorSpeed->value()* -1), ui->hsMotorSpeed->value()); }
void main() { // uint32 ms; // uint32 now; // uint8 cmdrAlive = 0; // Here we define what pins we will be using for PWM. // uint8 CODE pwmPins[] = {ptrGunMotor->pwmpin}; uint8 CODE pwmPins[] = {11}; MOTOR gunMotor = MAKE_MOTOR_3_PIN(pwmPins[0], 12, 13); //(PWM, B, A) MOTOR *ptrGunMotor = &gunMotor; // setDigitalOutput(param_arduino_DTR_pin, LOW); // ioTxSignals(0); // Initialize UARTs uart0Init(); uart0SetBaudRate(param_baud_rate_UART); uart1Init(); uart1SetBaudRate(param_baud_rate_XBEE); pwmStart((uint8 XDATA *)pwmPins, sizeof(pwmPins), 10000); guns_firing_duration = 125; // time in ms gunbutton = zFALSE; solenoid_on_duration = 80; // time in ms solenoidbutton = zFALSE; laserbutton = zFALSE; systemInit(); // Initialize other stuff index_cmdr = -1; /// MAIN LOOP /// while(1) { // updateSerialMode(); boardService(); updateLeds(); errorService(); // cmdr counts down from CMDR_ALIVE_CNT by -1 whenever no packets are received? cmdrAlive = (uint8) CLAMP(cmdrAlive + CmdrReadMsgs(), 0, CMDR_ALIVE_CNT); // ms = getMs(); // Get current time in ms // now = getMs(); // now = ms % (uint32)10000; // 10 sec for a full swing // if(now >= (uint16)5000){ // Goes from 0ms...5000ms // now = (uint16)10000 - now; // then 5000ms...0ms // } // speed = interpolate(now, 0, 5000, 100, 900); if (laserbutton == zTRUE && cmdrAlive > 0){ // uart0TxSendByte('L'); setDigitalOutput(param_laser_pin, HIGH); } else {setDigitalOutput(param_laser_pin, LOW);} //FIRE THE GUNS!!!!! //Resets timer while gunbutton is held down. if (gunbutton == zTRUE){ // uart0TxSendByte('Z'); guns_firing = zTRUE; setMotorSpeed(ptrGunMotor, -60); //NOTE: (7.2 / 12.6) * 127 = 72.5714286 guns_firing_start_time = getMs(); } //Check whether to stop firing guns if (guns_firing && clockHasElapsed(guns_firing_start_time, guns_firing_duration)){ // uart0TxSendByte('X'); guns_firing = zFALSE; setMotorSpeed(ptrGunMotor, 0); //NOTE: (7.2 / 12.6) * 127 = 72.5714286 guns_firing_start_time = getMs(); } //Activate solenoid for hopup feed unjammer if (solenoidbutton == zTRUE){ // uart0TxSendByte('Z'); solenoid_on = zTRUE; setDigitalOutput(10, HIGH); solenoid_on_start_time = getMs(); } //Check whether to disable solenoid if (solenoid_on && clockHasElapsed(solenoid_on_start_time, solenoid_on_duration)){ // uart0TxSendByte('X'); solenoid_on = zFALSE; setDigitalOutput(10, LOW); solenoid_on_start_time = getMs(); } delayMs(5); } }
int main(void){ SYSTEMConfigPerformance(10000000); enableInterrupts(); initTimer1(); initTimer2(); initTimer45(); initSW1(); initLCD(); initPWM(); initADC(); setMotorDirection(M1, 1); setMotorDirection(M2, 1); while(1){ //Lab3 Part1 switch(state){ case forward: prevstate = forward; ADCbuffer = getADCbuffer(); if((dispVolt < ADCbuffer) && ((dispVolt + 1) <= ADCbuffer)){//to reduce excessive LCD updates printVoltage(ADCbuffer); dispVolt = ADCbuffer; } else if((dispVolt > ADCbuffer) && ((dispVolt - 1) >= ADCbuffer)){ printVoltage(ADCbuffer); dispVolt = ADCbuffer; } moveCursorLCD(1,0); printStringLCD("forward "); if(remap == 1){ setMotorDirection(M1,1); setMotorDirection(M2,1); delayUs(1000); remap = 0; } setMotorSpeed(ADCbuffer, direction); break; case backward: prevstate = backward; ADCbuffer = getADCbuffer(); if((dispVolt < ADCbuffer) && ((dispVolt + 1) <= ADCbuffer)){//to reduce excessive LCD updates printVoltage(ADCbuffer); dispVolt = ADCbuffer; } else if((dispVolt > ADCbuffer) && ((dispVolt - 1) >= ADCbuffer)){ printVoltage(ADCbuffer); dispVolt = ADCbuffer; } moveCursorLCD(1,0); printStringLCD("backward"); if(remap == 1){ setMotorDirection(M1,0); setMotorDirection(M2,0); delayUs(1000); remap = 0; } setMotorSpeed(ADCbuffer, direction); break; case idle: unmapPins(); ADCbuffer = getADCbuffer(); if((dispVolt < ADCbuffer) && ((dispVolt + 1) <= ADCbuffer)){//to reduce excessive LCD updates printVoltage(ADCbuffer); dispVolt = ADCbuffer; } else if((dispVolt > ADCbuffer) && ((dispVolt - 1) >= ADCbuffer)){ printVoltage(ADCbuffer); dispVolt = ADCbuffer; } moveCursorLCD(1,0); printStringLCD("Idle "); delayUs(1000); break; } } return 0; }
void uTurn() { setMotorSpeed(150,0); setMotorSpeed(-150,1); delay(400); }
//turns the robot void turnMotor(int amount) { setMotorSpeed(leftMotorSpeed - amount, 0); setMotorSpeed(rightMotorSpeed + amount, 1); }
void stopMotors(){ setMotorSpeed(motorB, 0); setMotorSpeed(motorC, 0); }
void moveForward(int speed){ resetMotorEncoder(motorB); resetMotorEncoder(motorC); setMotorSpeed(motorB, speed); setMotorSpeed(motorC, speed); }
void turnRight(int speed){ resetMotorEncoder(motorB); resetMotorEncoder(motorC); setMotorSpeed(motorB, speed); setMotorSpeed(motorC, 0); }
void pimobileSetLeftMotorSpeed( short speed ) { setMotorSpeed( GPIO_LEFT_PWM, speed ); }
void stop(int pause) { setMotorSpeed(0,0, BRAKE, BRAKE); delay(pause); }
void pimobileSetRightMotorSpeed( short speed ) { setMotorSpeed( GPIO_RIGHT_PWM, speed ); }
int locateCandle(int speed, bool turnRight) { int found = 0; int distance = TURN90; int maxVal = 0; int angleAtMaxVal; if (turnRight) { setMotorSpeed(speed,speed, FORWARD, BACKWARD); } else{ setMotorSpeed(speed,speed, BACKWARD, FORWARD); } int prevVal = checkOdometry(); while (distance > 0){ if(turnRight) { int val = max(analogRead(UV_LEFT_PIN),analogRead(UV_RIGHT_PIN)); if(val > maxVal){ maxVal = val; angleAtMaxVal = distance; } } else { int val = max(analogRead(UV_LEFT_PIN),analogRead(UV_RIGHT_PIN)); if(val > maxVal){ maxVal = val; angleAtMaxVal = distance; } } int val = checkOdometry(); if (prevVal != val){ distance--; prevVal = val; } } if (maxVal < UV_THRESH) { if (turnRight) turnRight90(); else turnLeft90(); return NO_CANDLE; } stop(); //reverse direction if (turnRight) { turnLeftUntil(angleAtMaxVal); } else{ turnRightUntil(angleAtMaxVal); } // put candle in middle of sensors while (true) { if(turnRight) { Serial.print("turnRight ");Serial.print(analogRead(UV_LEFT_PIN));Serial.print(" ");Serial.println(analogRead(UV_RIGHT_PIN)); if (analogRead(UV_RIGHT_PIN)>= analogRead(UV_LEFT_PIN)) { break; } } else { Serial.print("turnLeft ");Serial.print(analogRead(UV_LEFT_PIN));Serial.print(" ");Serial.println(analogRead(UV_RIGHT_PIN)); if (analogRead(UV_LEFT_PIN)>= analogRead(UV_RIGHT_PIN)) { break; } } } stop(); if (angleAtMaxVal <= .1 * TURN90) return TO_SIDE; else if (angleAtMaxVal <= .8 * TURN90) return KIDDIE_CORNER; else return STRAIGHT_AHEAD; }
void Controllingroomba::on_pbDriveForward_pressed() { emit setMotorSpeed(ui->hsMotorSpeed->value(), ui->hsMotorSpeed->value()); }
void stopAndWait() { setMotorSpeed(LWheel, 0); setMotorSpeed(RWheel, 0); sleep(250); }
void Controllingroomba::on_pbDriveBackward_pressed() { emit setMotorSpeed((ui->hsMotorSpeed->value() * -1), (ui->hsMotorSpeed->value()* -1)); }
void avoid() { int i = 0; //Turn setMotorSpeed(RWheel, 8); setMotorSpeed(LWheel, -8); sleep(2300); //Go straight for(i = 0; true; i++) { displayTextLine(1, "%d", i); stopAndWait(); driveToTarget(300); stopAndWait(); setMotorSpeed(RWheel, -8); setMotorSpeed(LWheel, 8); sleep(2300); stopAndWait(); if(leftUS > 15 && rightUS > 15) { break; } else { setMotorSpeed(RWheel, 8); setMotorSpeed(LWheel, -8); sleep(2300); } } while(true) { stopAndWait(); driveToTarget(400); stopAndWait(); setMotorSpeed(RWheel, -8); setMotorSpeed(LWheel, 8); sleep(2300); stopAndWait(); if(leftUS > 15 && rightUS > 15) { break; } else { setMotorSpeed(RWheel, 8); setMotorSpeed(LWheel, -8); sleep(2300); } } driveToTarget(300*i); stopAndWait(); setMotorSpeed(RWheel, 8); setMotorSpeed(LWheel, -8); sleep(2300); }
void Controllingroomba::on_pbDriveRight_pressed() { emit setMotorSpeed(ui->hsMotorSpeed->value(), (ui->hsMotorSpeed->value()* -1)); }
int main() { // Handle Ctrl-C quit signal(SIGINT, sig_handler); // mraa::Gpio *chipSelect = new mraa::Gpio(10); // chipSelect->dir(mraa::DIR_OUT); // chipSelect->write(1); // mraa::Spi *spi = new mraa::Spi(0); // spi->bitPerWord(32); // char rxBuf[2]; // char writeBuf[4]; // unsigned int sensorRead = 0x20000000; // writeBuf[0] = sensorRead & 0xff; // writeBuf[1] = (sensorRead >> 8) & 0xff; // writeBuf[2] = (sensorRead >> 16) & 0xff; // writeBuf[3] = (sensorRead >> 24) & 0xff; // float total = 0; // struct timeval tv; // int init = 0; // float rf; signal(SIGINT, sig_handler); //Motor Stuff mraa::Pwm pwm = mraa::Pwm(9); pwm.write(0.0); pwm.enable(true); //assert(pwm != NULL); mraa::Gpio dir = mraa::Gpio(8); //assert(dir != NULL); dir.dir(mraa::DIR_OUT); dir.write(0); mraa::Pwm pwm2 = mraa::Pwm(6); pwm2.write(0.0); pwm2.enable(true); //assert(pwm2 != NULL); mraa::Gpio dir2 = mraa::Gpio(5); //assert(dir != NULL); dir2.dir(mraa::DIR_OUT); dir2.write(0); mraa::Aio aioBackInfrared = mraa::Aio(BACK_INFRARED_PIN); mraa::Aio aioFrontInfrared = mraa::Aio(FRONT_INFRARED_PIN); mraa::Aio aioHeadInfrared = mraa::Aio(HEAD_INFRARED_PIN); float speed = .1; float desiredAngle = 0.0; float diffAngle = 0.0; float integral = 0; float power = 0; float derivative = 0; float timeBetweenReadings = 0; float gyroBias = 1.0; float forwardBias = 0.15; float P_CONSTANT = 25; float I_CONSTANT = 0; float D_CONSTANT = -1; float backDistance = 3.0; float frontDistance = 3.0; float P_CONSTANT_WALL_FOLLOWER = .05; while (running) { float backInfraredReading = aioBackInfrared.read(); float frontInfraredReading = aioFrontInfrared.read(); float headInfraredReading = aioHeadInfrared.read(); printf("Infra readings: back: %f, front: %f, head: %f\n", backInfraredReading, frontInfraredReading, headInfraredReading); backDistance = (backDistance * alpha) + (infraReadingToDistanceBack(backInfraredReading) * (1.0 - alpha)); frontDistance = (frontDistance * alpha) + (infraReadingToDistanceFront(frontInfraredReading) * (1.0 - alpha) * .94); printf("Distances: Front: %f, Back: %f\n", backDistance, frontDistance); float infraAngle = angleFromWall(backDistance, frontDistance); printf("estimated angle: %f\n", infraAngle); // power = speed * ((P_CONSTANT * diffAngle / 360.0) + (I_CONSTANT * integral) + (D_CONSTANT * derivative / 180.0)); //make sure to convert angles > 360 to proper angles if (!isnan(infraAngle)){ power = speed * (P_CONSTANT_WALL_FOLLOWER * infraAngle); } // if (fabs(infraAngle) < 5.0){ // power = 0; // } if (power > .3) { power = .3; } else if (power < -.3) { power = -.3; } if (headInfraredReading > 500.0){ setMotorSpeed(pwm, dir, power + forwardBias); setMotorSpeed(pwm2, dir2, power - forwardBias); } else { printf("Power reduced!\n"); power = .15; setMotorSpeed(pwm, dir, power); setMotorSpeed(pwm2, dir2, power); usleep(1000 * 20); } printf("Set power to: %f\n", power); usleep(1000 * 10); } return 0; }
/// close or open claw void clawClose(bool close) { setMotorSpeed(clawMotor, (close ? 1 * CLAW_SPEED : -1) * CLAW_SPEED); sleep(200); setMotorSpeed(clawMotor, 0); }
void turnLeft(int speed){ resetMotorEncoder(motorB); resetMotorEncoder(motorC); setMotorSpeed(motorB, 0); setMotorSpeed(motorC, speed); }