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:
- chris1996
- Date:
- 2017-05-10
- Revision:
- 17:df86b0e2bb40
- Parent:
- 15:e7fc3271bf53
- Child:
- 18:f2a516ad24eb
File content as of revision 17:df86b0e2bb40:
//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); //Serial //Line Detector SPI spi(D11,D12,D13);//SPI interface DigitalOut cs(D10);// chip select //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 = 120; //set cds to 0 for(int i=0; i<8; i++) _cds[i] = 0; //cs = 0; // //Averaging // spi.write(0x3C); // cs = 1; wait_ms(200); 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]; } //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; } //Motor Control #include "motordriver.h" Motor A(D6, D5, D4, 1); // pwm, fwd, rev, can break Motor B(D3, D7, D2, 1); // pwm, fwd, rev, can break void StopMotors(void) { A.stop(1); B.stop(1); } void TurnRight() { A.speed(1); B.stop(1); wait(.3); } void TurnLeft() { B.speed(1); A.stop(1); wait(.3); } void DriveStraight() { A.speed(1); //Right B.speed(1); //Left } void DriveSquare() { DriveStraight(); wait(1); TurnLeft(); DriveStraight(); wait(1); TurnLeft(); DriveStraight(); wait(1); TurnLeft(); DriveStraight(); wait(1); TurnLeft(); } //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: for(int i=0; i<8; i++) { scan(i); _larray[i] = scan(i); //LF track using m and n //Forget which ones work exactly int SpeedOnLine = .75; int SpeedOffLine = .5; int m = 1; int n = 2; int OnLineLeft = _larray[m]; int OnLineRight = _larray[n]; if((OnLineLeft && OnLineRight) > 200){ A.speed(SpeedOnLine); B.speed(SpeedOnLine); } if((OnLineLeft > 200) && (OnLineRight < 200)){ A.speed(SpeedOffLine); B.speed(SpeedOnLine); } if((OnLineRight > 200) && (OnLineLeft < 200)){ A.speed(SpeedOnLine); B.speed(SpeedOffLine); } } break; //OA case 3: break; //LA case 4: scan(10); _cds[0] = scan(10); scan(8); _cds[1] = scan(8); float speedLeft = (_cds[0]-200)/200; float speedRight = (_cds[1]-200)/200; if (speedLeft < 0) { B.speed(speedLeft); } if (speedRight < 0) { A.speed(speedRight); } if ((speedRight && speedLeft) >= 0) { StopMotors(); } break; //TRC case 5: DriveStraight(); break; //WiiC case 6: DriveSquare(); break; } return (int)(ain.read()*100); }