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);
}
