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
Diff: Classes/RobotMVC/RobotModel.cpp
- Revision:
- 15:e7fc3271bf53
- Parent:
- 11:10a7bb4bc714
- Child:
- 17:df86b0e2bb40
--- a/Classes/RobotMVC/RobotModel.cpp Wed May 10 02:51:54 2017 +0000 +++ b/Classes/RobotMVC/RobotModel.cpp Wed May 10 03:21:35 2017 +0000 @@ -36,8 +36,8 @@ led_orange = 0; led_red = 0; led_blue = 0; - - //Line Detector Setup + + //Line Detector Setup // Chip must be deselected cs = 1; // Setup the spi for 8 bit data, high steady state clock, @@ -55,7 +55,7 @@ // //Averaging // spi.write(0x3C); // cs = 1; - + wait_ms(200); led_orange = 1; led_red = 1; @@ -63,24 +63,27 @@ } //read the indicated light sensor -int RobotModel::checkLight(int x){ +int RobotModel::checkLight(int x) +{ return _cds[x]; } //read bit b of the line array -int RobotModel::checkLine(int b){ +int RobotModel::checkLine(int b) +{ return _larray[b]; } //scan the ADC on channel n -int RobotModel::scan(int 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){ + switch(n) { default: case 0: spi.write(0x84); @@ -124,6 +127,47 @@ 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() { @@ -131,21 +175,37 @@ led_green = 0; else led_green = !led_green; - - if(_mode == 2) - { - for(int i=0; i<8; i++) - { - scan(i); - _larray[i] = scan(i); - } + + switch(_mode) { + default: + //Reset + case 0: + StopMotors(); + break; + //LF + case 2: + for(int i=0; i<8; i++) { + scan(i); + _larray[i] = scan(i); + } + break; + //OA + case 3: + break; + //LA + case 4: + scan(10); + _cds[0] = scan(10); + scan(8); + _cds[1] = scan(8); + break; + //TRC + case 5: + break; + //WiiC + case 6: + DriveSquare(); + break; } - if(_mode == 4) - { - scan(10); - _cds[0] = scan(10); - scan(8); - _cds[1] = scan(8); - } - return 0; + return (int)(ain.read()*100); } \ No newline at end of file