Dr. Davis and Dr. Dyer special studies robotics project
Dependencies: BSP_DISCO_F469NI LCD_DISCO_F469NI TS_DISCO_F469NI mbed Motordriver
Fork of Configurable_Robots by
Classes/RobotMVC/RobotModel.cpp
- Committer:
- blu12758
- Date:
- 2017-05-11
- Revision:
- 21:ee2b617cc0e6
- Parent:
- 20:4173e6b7adde
File content as of revision 21:ee2b617cc0e6:
//OU Configurable Robot Project //Spring 2017 //William Bonner #include "RobotModel.h" //Pin definitions //Onboard LEDs DigitalOut led_green(LED1); DigitalOut led_orange(LED2); DigitalOut led_red(LED3); DigitalOut led_blue(LED4); //Input Pins AnalogIn ain(A0); //Sonar DigitalOut trig(A1); AnalogIn echo(A2); //Serial //Line Detector SPI spi(D11,D12,D13);//SPI interface DigitalOut cs(D10);// chip select //Motors Motor RightMotor(D6, D5, D4, 1); // pwm, fwd, rev, can break Motor LeftMotor(D3, D7, D2, 1); // pwm, fwd, rev, can break //Constructors/Destructors RobotModel::~RobotModel() { //#TODO } RobotModel::RobotModel() { _mode = 0; } //initialize the robot's hardware void RobotModel::init() { led_green = 0; led_orange = 0; led_red = 0; led_blue = 0; //Line Detector Setup // Chip must be deselected cs = 1; // Setup the spi for 8 bit data, high steady state clock, // second edge capture, with a 1MHz clock rate spi.format(8,0); spi.frequency(1000000); //set line array to 0 for(int i=0; i<8; i++) _larray[i] = 0; _threshold = 200; //set cds to 0 for(int i=0; i<8; i++) _cds[i] = 0; trig.write(0); speedLeft = 0; speedRight = 0; wait_ms(100); led_orange = 1; led_red = 1; led_blue = 1; } //read the indicated light sensor int RobotModel::checkLight(int x) { return _cds[x]; } //read bit b of the line array int RobotModel::checkLine(int b) { return _larray[b]>_threshold ? 1 : 0; } //scan the ADC on channel n int RobotModel::scan(int n) { // Select the device by seting chip select low cs = 0; spi.write(0x18); //Scan channel N //Command bits -- 1 CHS3 CHS2 CHS1 CHS0 1 1 0 //CHSX is the binary for N switch(n) { default: case 0: spi.write(0x84); break; case 1: spi.write(0x8c); break; case 2: spi.write(0x94); break; case 3: spi.write(0x9C); break; case 4: spi.write(0xA4); break; case 5: spi.write(0xAC); break; case 6: spi.write(0xB4); break; case 7: spi.write(0xBC); break; case 8: spi.write(0xC4); break; case 9: spi.write(0xCC); break; case 10: spi.write(0xD4); break; } int temp = spi.write(0x00) << 4; temp |= spi.write(0x00) >> 4; spi.write(0x18); // Deselect the device cs = 1; return temp; } void RobotModel::StopMotors(void) { RightMotor.stop(1); LeftMotor.stop(1); } void RobotModel::TurnRight() { RightMotor.speed(1); LeftMotor.stop(1); wait(.3875); } void RobotModel::TurnLeft() { LeftMotor.speed(1); RightMotor.stop(1); wait(.3875); } void RobotModel::DriveStraight() { RightMotor.speed(.5); //Right LeftMotor.speed(.572); //Left } void RobotModel::DriveStraightMode(int ModeSetting) { switch(ModeSetting) { default: //Medium is default case 0: RightMotor.speed(.5); //Right LeftMotor.speed(.572); //Left //Slow case 1: RightMotor.speed(.25); //Right LeftMotor.speed(.2672); //Left //Fast case 2: RightMotor.speed(.75); //Right LeftMotor.speed(.8172); //Left //Max case 3: RightMotor.speed(1); //Right LeftMotor.speed(1); //Left } } void TurnDegrees(int Degrees,int TurnDir) //Degrees to turn, TurnDir = 1 for left, 0 for right { //Roughly .004 seconds turns 1 degree float DegreesFactor = .0043055556; //in seconds float TurnTime = (float)DegreesFactor*Degrees; if(TurnDir == 0) { RightMotor.stop(1); LeftMotor.speed(1); wait(TurnTime); } else { LeftMotor.stop(1); RightMotor.speed(1); wait(TurnTime); } } void RobotModel::DriveSquare() { DriveStraight(); wait(1); TurnLeft(); DriveStraight(); wait(1); TurnLeft(); DriveStraight(); wait(1); TurnLeft(); DriveStraight(); wait(1); TurnLeft(); } float RobotModel::LeftSpeed(){ return speedLeft; } float RobotModel::RightSpeed(){ return speedRight; } //update the model based on the mode int RobotModel::update() { if(_mode == 0) led_green = 0; else led_green = !led_green; switch(_mode) { default: //Reset case 0: StopMotors(); break; //LF case 2: //Read line sensors for(int i=0; i<8; i++) { scan(i); _larray[i] = scan(i); } //Calculate Line Location float p = 0; float Gp = 0.5f; int width = 0; for(int i=0; i<8; i++) { int temp = checkLine(i); p += temp*i; width += temp>0 ? 1 : 0; } p = Gp*(p/width)/7; float speedLeft = 0; float speedRight = 0; if((width == 8) || (width == 7)){ StopMotors(); return -1; }else if(width == 0){ LeftMotor.speed(Gp*.5172); RightMotor.speed(Gp*.5); return 0; }else{ speedLeft = Gp-p; speedRight = p*Gp; } LeftMotor.speed(speedRight); RightMotor.speed(speedLeft*0.5f); return (int)(p*100); //OF case 3: //sonar_t.reset(); // //ping sensor // trig.write(1); // wait_us(10); // trig.write(0); // while(echo){} // sonar_t.start(); // while(!echo){} // sonar_t.stop(); // float time_us = sonar_t.read_us(); float volts = echo.read(); if(volts <= .4){ LeftMotor.speed(.15); RightMotor.speed(-.15); }else{ DriveStraight(); } return (int)(volts * 100); //LA case 4: //Scan sensors scan(10); _cds[0] = scan(10); scan(8); _cds[1] = scan(8); //Convert readings speedLeft = (float)(_cds[0]-200)/200; speedRight = (float)(_cds[1]-200)/200; //Check light levels if ((speedLeft < 0) && (speedRight < 0)) { LeftMotor.speed(speedLeft); RightMotor.speed(speedRight); }else if(speedRight < 0){ LeftMotor.stop(1); RightMotor.speed(speedRight); }else if (speedLeft < 0) { LeftMotor.speed(speedLeft); RightMotor.stop(1); }else{ StopMotors(); } return 4; //TRC case 5: DriveStraight(); break; //WiiC case 6: DriveSquare(); break; } return (int)(ain.read()*100); }