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@15:e7fc3271bf53, 2017-05-10 (annotated)
- Committer:
- chris1996
- Date:
- Wed May 10 03:21:35 2017 +0000
- Revision:
- 15:e7fc3271bf53
- Parent:
- 11:10a7bb4bc714
- Child:
- 17:df86b0e2bb40
Updated;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
blu12758 | 7:0f8c3dfbbb86 | 1 | //OU Configurable Robot Project |
blu12758 | 7:0f8c3dfbbb86 | 2 | //Spring 2017 |
blu12758 | 7:0f8c3dfbbb86 | 3 | //William Bonner |
blu12758 | 7:0f8c3dfbbb86 | 4 | |
blu12758 | 7:0f8c3dfbbb86 | 5 | #include "RobotModel.h" |
blu12758 | 7:0f8c3dfbbb86 | 6 | |
blu12758 | 10:4dd8b18e07d0 | 7 | //Pin definitions |
blu12758 | 10:4dd8b18e07d0 | 8 | //Onboard LEDs |
blu12758 | 10:4dd8b18e07d0 | 9 | DigitalOut led_green(LED1); |
blu12758 | 10:4dd8b18e07d0 | 10 | DigitalOut led_orange(LED2); |
blu12758 | 10:4dd8b18e07d0 | 11 | DigitalOut led_red(LED3); |
blu12758 | 10:4dd8b18e07d0 | 12 | DigitalOut led_blue(LED4); |
blu12758 | 10:4dd8b18e07d0 | 13 | |
blu12758 | 10:4dd8b18e07d0 | 14 | //Input Pins |
blu12758 | 10:4dd8b18e07d0 | 15 | AnalogIn ain(A0); |
blu12758 | 10:4dd8b18e07d0 | 16 | |
blu12758 | 11:10a7bb4bc714 | 17 | //Serial |
blu12758 | 11:10a7bb4bc714 | 18 | //Line Detector |
blu12758 | 11:10a7bb4bc714 | 19 | SPI spi(D11,D12,D13);//SPI interface |
blu12758 | 11:10a7bb4bc714 | 20 | DigitalOut cs(D10);// chip select |
blu12758 | 7:0f8c3dfbbb86 | 21 | |
blu12758 | 7:0f8c3dfbbb86 | 22 | //Constructors/Destructors |
blu12758 | 7:0f8c3dfbbb86 | 23 | RobotModel::~RobotModel() |
blu12758 | 7:0f8c3dfbbb86 | 24 | { |
blu12758 | 7:0f8c3dfbbb86 | 25 | //#TODO |
blu12758 | 7:0f8c3dfbbb86 | 26 | } |
blu12758 | 7:0f8c3dfbbb86 | 27 | RobotModel::RobotModel() |
blu12758 | 7:0f8c3dfbbb86 | 28 | { |
blu12758 | 7:0f8c3dfbbb86 | 29 | _mode = 0; |
blu12758 | 10:4dd8b18e07d0 | 30 | } |
blu12758 | 10:4dd8b18e07d0 | 31 | |
blu12758 | 10:4dd8b18e07d0 | 32 | //initialize the robot's hardware |
blu12758 | 10:4dd8b18e07d0 | 33 | void RobotModel::init() |
blu12758 | 10:4dd8b18e07d0 | 34 | { |
blu12758 | 10:4dd8b18e07d0 | 35 | led_green = 0; |
blu12758 | 10:4dd8b18e07d0 | 36 | led_orange = 0; |
blu12758 | 10:4dd8b18e07d0 | 37 | led_red = 0; |
blu12758 | 10:4dd8b18e07d0 | 38 | led_blue = 0; |
chris1996 | 15:e7fc3271bf53 | 39 | |
chris1996 | 15:e7fc3271bf53 | 40 | //Line Detector Setup |
blu12758 | 11:10a7bb4bc714 | 41 | // Chip must be deselected |
blu12758 | 11:10a7bb4bc714 | 42 | cs = 1; |
blu12758 | 11:10a7bb4bc714 | 43 | // Setup the spi for 8 bit data, high steady state clock, |
blu12758 | 11:10a7bb4bc714 | 44 | // second edge capture, with a 1MHz clock rate |
blu12758 | 11:10a7bb4bc714 | 45 | spi.format(8,0); |
blu12758 | 11:10a7bb4bc714 | 46 | spi.frequency(1000000); |
blu12758 | 11:10a7bb4bc714 | 47 | //set line array to 0 |
blu12758 | 11:10a7bb4bc714 | 48 | for(int i=0; i<8; i++) |
blu12758 | 11:10a7bb4bc714 | 49 | _larray[i] = 0; |
blu12758 | 11:10a7bb4bc714 | 50 | _threshold = 120; |
blu12758 | 11:10a7bb4bc714 | 51 | //set cds to 0 |
blu12758 | 11:10a7bb4bc714 | 52 | for(int i=0; i<8; i++) |
blu12758 | 11:10a7bb4bc714 | 53 | _cds[i] = 0; |
blu12758 | 11:10a7bb4bc714 | 54 | //cs = 0; |
blu12758 | 11:10a7bb4bc714 | 55 | // //Averaging |
blu12758 | 11:10a7bb4bc714 | 56 | // spi.write(0x3C); |
blu12758 | 11:10a7bb4bc714 | 57 | // cs = 1; |
chris1996 | 15:e7fc3271bf53 | 58 | |
blu12758 | 10:4dd8b18e07d0 | 59 | wait_ms(200); |
blu12758 | 10:4dd8b18e07d0 | 60 | led_orange = 1; |
blu12758 | 10:4dd8b18e07d0 | 61 | led_red = 1; |
blu12758 | 10:4dd8b18e07d0 | 62 | led_blue = 1; |
blu12758 | 10:4dd8b18e07d0 | 63 | } |
blu12758 | 10:4dd8b18e07d0 | 64 | |
blu12758 | 11:10a7bb4bc714 | 65 | //read the indicated light sensor |
chris1996 | 15:e7fc3271bf53 | 66 | int RobotModel::checkLight(int x) |
chris1996 | 15:e7fc3271bf53 | 67 | { |
blu12758 | 11:10a7bb4bc714 | 68 | return _cds[x]; |
blu12758 | 11:10a7bb4bc714 | 69 | } |
blu12758 | 11:10a7bb4bc714 | 70 | |
blu12758 | 11:10a7bb4bc714 | 71 | //read bit b of the line array |
chris1996 | 15:e7fc3271bf53 | 72 | int RobotModel::checkLine(int b) |
chris1996 | 15:e7fc3271bf53 | 73 | { |
blu12758 | 11:10a7bb4bc714 | 74 | return _larray[b]; |
blu12758 | 11:10a7bb4bc714 | 75 | } |
blu12758 | 11:10a7bb4bc714 | 76 | |
blu12758 | 11:10a7bb4bc714 | 77 | //scan the ADC on channel n |
chris1996 | 15:e7fc3271bf53 | 78 | int RobotModel::scan(int n) |
chris1996 | 15:e7fc3271bf53 | 79 | { |
blu12758 | 11:10a7bb4bc714 | 80 | // Select the device by seting chip select low |
blu12758 | 11:10a7bb4bc714 | 81 | cs = 0; |
blu12758 | 11:10a7bb4bc714 | 82 | spi.write(0x18); |
blu12758 | 11:10a7bb4bc714 | 83 | //Scan channel N |
blu12758 | 11:10a7bb4bc714 | 84 | //Command bits -- 1 CHS3 CHS2 CHS1 CHS0 1 1 0 |
blu12758 | 11:10a7bb4bc714 | 85 | //CHSX is the binary for N |
chris1996 | 15:e7fc3271bf53 | 86 | switch(n) { |
blu12758 | 11:10a7bb4bc714 | 87 | default: |
blu12758 | 11:10a7bb4bc714 | 88 | case 0: |
blu12758 | 11:10a7bb4bc714 | 89 | spi.write(0x84); |
blu12758 | 11:10a7bb4bc714 | 90 | break; |
blu12758 | 11:10a7bb4bc714 | 91 | case 1: |
blu12758 | 11:10a7bb4bc714 | 92 | spi.write(0x8c); |
blu12758 | 11:10a7bb4bc714 | 93 | break; |
blu12758 | 11:10a7bb4bc714 | 94 | case 2: |
blu12758 | 11:10a7bb4bc714 | 95 | spi.write(0x94); |
blu12758 | 11:10a7bb4bc714 | 96 | break; |
blu12758 | 11:10a7bb4bc714 | 97 | case 3: |
blu12758 | 11:10a7bb4bc714 | 98 | spi.write(0x9C); |
blu12758 | 11:10a7bb4bc714 | 99 | break; |
blu12758 | 11:10a7bb4bc714 | 100 | case 4: |
blu12758 | 11:10a7bb4bc714 | 101 | spi.write(0xA4); |
blu12758 | 11:10a7bb4bc714 | 102 | break; |
blu12758 | 11:10a7bb4bc714 | 103 | case 5: |
blu12758 | 11:10a7bb4bc714 | 104 | spi.write(0xAC); |
blu12758 | 11:10a7bb4bc714 | 105 | break; |
blu12758 | 11:10a7bb4bc714 | 106 | case 6: |
blu12758 | 11:10a7bb4bc714 | 107 | spi.write(0xB4); |
blu12758 | 11:10a7bb4bc714 | 108 | break; |
blu12758 | 11:10a7bb4bc714 | 109 | case 7: |
blu12758 | 11:10a7bb4bc714 | 110 | spi.write(0xBC); |
blu12758 | 11:10a7bb4bc714 | 111 | break; |
blu12758 | 11:10a7bb4bc714 | 112 | case 8: |
blu12758 | 11:10a7bb4bc714 | 113 | spi.write(0xC4); |
blu12758 | 11:10a7bb4bc714 | 114 | break; |
blu12758 | 11:10a7bb4bc714 | 115 | case 9: |
blu12758 | 11:10a7bb4bc714 | 116 | spi.write(0xCC); |
blu12758 | 11:10a7bb4bc714 | 117 | break; |
blu12758 | 11:10a7bb4bc714 | 118 | case 10: |
blu12758 | 11:10a7bb4bc714 | 119 | spi.write(0xD4); |
blu12758 | 11:10a7bb4bc714 | 120 | break; |
blu12758 | 11:10a7bb4bc714 | 121 | } |
blu12758 | 11:10a7bb4bc714 | 122 | int temp = spi.write(0x00) << 4; |
blu12758 | 11:10a7bb4bc714 | 123 | temp |= spi.write(0x00) >> 4; |
blu12758 | 11:10a7bb4bc714 | 124 | spi.write(0x18); |
blu12758 | 11:10a7bb4bc714 | 125 | // Deselect the device |
blu12758 | 11:10a7bb4bc714 | 126 | cs = 1; |
blu12758 | 11:10a7bb4bc714 | 127 | return temp; |
blu12758 | 11:10a7bb4bc714 | 128 | } |
blu12758 | 11:10a7bb4bc714 | 129 | |
chris1996 | 15:e7fc3271bf53 | 130 | //Motor Control |
chris1996 | 15:e7fc3271bf53 | 131 | #include "motordriver.h" |
chris1996 | 15:e7fc3271bf53 | 132 | |
chris1996 | 15:e7fc3271bf53 | 133 | |
chris1996 | 15:e7fc3271bf53 | 134 | Motor A(D6, D5, D4, 1); // pwm, fwd, rev, can break |
chris1996 | 15:e7fc3271bf53 | 135 | Motor B(D3, D7, D2, 1); // pwm, fwd, rev, can break |
chris1996 | 15:e7fc3271bf53 | 136 | |
chris1996 | 15:e7fc3271bf53 | 137 | void StopMotors(void){ |
chris1996 | 15:e7fc3271bf53 | 138 | A.stop(1); |
chris1996 | 15:e7fc3271bf53 | 139 | B.stop(1); |
chris1996 | 15:e7fc3271bf53 | 140 | } |
chris1996 | 15:e7fc3271bf53 | 141 | |
chris1996 | 15:e7fc3271bf53 | 142 | void TurnRight(){ |
chris1996 | 15:e7fc3271bf53 | 143 | A.speed(1); |
chris1996 | 15:e7fc3271bf53 | 144 | B.stop(1); |
chris1996 | 15:e7fc3271bf53 | 145 | wait(.3); |
chris1996 | 15:e7fc3271bf53 | 146 | } |
chris1996 | 15:e7fc3271bf53 | 147 | void TurnLeft(){ |
chris1996 | 15:e7fc3271bf53 | 148 | B.speed(1); |
chris1996 | 15:e7fc3271bf53 | 149 | A.stop(1); |
chris1996 | 15:e7fc3271bf53 | 150 | wait(.3); |
chris1996 | 15:e7fc3271bf53 | 151 | } |
chris1996 | 15:e7fc3271bf53 | 152 | void DriveStraight(){ |
chris1996 | 15:e7fc3271bf53 | 153 | A.speed(1); //Right |
chris1996 | 15:e7fc3271bf53 | 154 | B.speed(1); //Left |
chris1996 | 15:e7fc3271bf53 | 155 | } |
chris1996 | 15:e7fc3271bf53 | 156 | void DriveSquare(){ |
chris1996 | 15:e7fc3271bf53 | 157 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 158 | wait(1); |
chris1996 | 15:e7fc3271bf53 | 159 | TurnLeft(); |
chris1996 | 15:e7fc3271bf53 | 160 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 161 | wait(1); |
chris1996 | 15:e7fc3271bf53 | 162 | TurnLeft(); |
chris1996 | 15:e7fc3271bf53 | 163 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 164 | wait(1); |
chris1996 | 15:e7fc3271bf53 | 165 | TurnLeft(); |
chris1996 | 15:e7fc3271bf53 | 166 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 167 | wait(1); |
chris1996 | 15:e7fc3271bf53 | 168 | TurnLeft(); |
chris1996 | 15:e7fc3271bf53 | 169 | } |
chris1996 | 15:e7fc3271bf53 | 170 | |
blu12758 | 10:4dd8b18e07d0 | 171 | //update the model based on the mode |
blu12758 | 10:4dd8b18e07d0 | 172 | int RobotModel::update() |
blu12758 | 10:4dd8b18e07d0 | 173 | { |
blu12758 | 10:4dd8b18e07d0 | 174 | if(_mode == 0) |
blu12758 | 10:4dd8b18e07d0 | 175 | led_green = 0; |
blu12758 | 10:4dd8b18e07d0 | 176 | else |
blu12758 | 10:4dd8b18e07d0 | 177 | led_green = !led_green; |
chris1996 | 15:e7fc3271bf53 | 178 | |
chris1996 | 15:e7fc3271bf53 | 179 | switch(_mode) { |
chris1996 | 15:e7fc3271bf53 | 180 | default: |
chris1996 | 15:e7fc3271bf53 | 181 | //Reset |
chris1996 | 15:e7fc3271bf53 | 182 | case 0: |
chris1996 | 15:e7fc3271bf53 | 183 | StopMotors(); |
chris1996 | 15:e7fc3271bf53 | 184 | break; |
chris1996 | 15:e7fc3271bf53 | 185 | //LF |
chris1996 | 15:e7fc3271bf53 | 186 | case 2: |
chris1996 | 15:e7fc3271bf53 | 187 | for(int i=0; i<8; i++) { |
chris1996 | 15:e7fc3271bf53 | 188 | scan(i); |
chris1996 | 15:e7fc3271bf53 | 189 | _larray[i] = scan(i); |
chris1996 | 15:e7fc3271bf53 | 190 | } |
chris1996 | 15:e7fc3271bf53 | 191 | break; |
chris1996 | 15:e7fc3271bf53 | 192 | //OA |
chris1996 | 15:e7fc3271bf53 | 193 | case 3: |
chris1996 | 15:e7fc3271bf53 | 194 | break; |
chris1996 | 15:e7fc3271bf53 | 195 | //LA |
chris1996 | 15:e7fc3271bf53 | 196 | case 4: |
chris1996 | 15:e7fc3271bf53 | 197 | scan(10); |
chris1996 | 15:e7fc3271bf53 | 198 | _cds[0] = scan(10); |
chris1996 | 15:e7fc3271bf53 | 199 | scan(8); |
chris1996 | 15:e7fc3271bf53 | 200 | _cds[1] = scan(8); |
chris1996 | 15:e7fc3271bf53 | 201 | break; |
chris1996 | 15:e7fc3271bf53 | 202 | //TRC |
chris1996 | 15:e7fc3271bf53 | 203 | case 5: |
chris1996 | 15:e7fc3271bf53 | 204 | break; |
chris1996 | 15:e7fc3271bf53 | 205 | //WiiC |
chris1996 | 15:e7fc3271bf53 | 206 | case 6: |
chris1996 | 15:e7fc3271bf53 | 207 | DriveSquare(); |
chris1996 | 15:e7fc3271bf53 | 208 | break; |
blu12758 | 11:10a7bb4bc714 | 209 | } |
chris1996 | 15:e7fc3271bf53 | 210 | return (int)(ain.read()*100); |
blu12758 | 7:0f8c3dfbbb86 | 211 | } |