Configurable Robots / Mbed 2 deprecated Configurable_Robots

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 Christopher Eubanks

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?

UserRevisionLine numberNew 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 }