Arend Peter Castelein / Alexa_Inventory

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   LSM9DS1_Library_cal Motordriver

main.cpp@5:507618748cb4, 2016-12-05 (annotated)

Committer:
hbindra3
Date:
Mon Dec 05 22:45:09 2016 +0000
Revision:
5:507618748cb4
Parent:
4:104acd180bb1
Child:
6:2fa349da33bb
motor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
apcastelein 0:67dc92eb6d5f 1 #include "mbed.h"
apcastelein 3:b65b394375b7 2 #include "LSM9DS1.h"
hbindra3 5:507618748cb4 3 #include "motordriver.h"
apcastelein 3:b65b394375b7 4 #define PI 3.14159
apcastelein 3:b65b394375b7 5
apcastelein 3:b65b394375b7 6 //IMU used for monitoring orientation
apcastelein 3:b65b394375b7 7 // - wired using i2c in cookbook example here
apcastelein 3:b65b394375b7 8 // https://developer.mbed.org/components/LSM9DS1-IMU/
apcastelein 3:b65b394375b7 9 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
hbindra3 5:507618748cb4 10 float initial_direction;
hbindra3 5:507618748cb4 11 Motor left(p21, p22, p23, 1);
hbindra3 5:507618748cb4 12 Motor right(p26, p25, p24, 1);
hbindra3 5:507618748cb4 13
apcastelein 3:b65b394375b7 14
apcastelein 4:104acd180bb1 15 //Color sensor, used to assist robot positioning
apcastelein 4:104acd180bb1 16 // - wired using i2c according to
apcastelein 4:104acd180bb1 17 // https://developer.mbed.org/users/raj1995/notebook/adafruit-tcs34725-rgb-color-sensor/
apcastelein 4:104acd180bb1 18 I2C color_sensor(p28, p27); //pins for I2C communication (SDA, SCL)
apcastelein 4:104acd180bb1 19 int color_addr = 41 << 1;
hbindra3 5:507618748cb4 20 int color_thresh = 4500;//threshold for green before it triggers
apcastelein 4:104acd180bb1 21 bool on_color = false;
hbindra3 5:507618748cb4 22 DigitalOut green(LED1);
apcastelein 4:104acd180bb1 23
apcastelein 3:b65b394375b7 24 //pc serial connection, for debugging purposes
apcastelein 3:b65b394375b7 25 Serial pc(USBTX, USBRX);
apcastelein 3:b65b394375b7 26
apcastelein 3:b65b394375b7 27 //initialize and calibrate IMU
hbindra3 5:507618748cb4 28 /*
apcastelein 3:b65b394375b7 29 void initIMU(){
apcastelein 3:b65b394375b7 30 IMU.begin();
apcastelein 3:b65b394375b7 31 if (!IMU.begin()) {
apcastelein 3:b65b394375b7 32 pc.printf("Failed to communicate with LSM9DS1.\n");
apcastelein 3:b65b394375b7 33 }
apcastelein 3:b65b394375b7 34 IMU.calibrate(1);
apcastelein 3:b65b394375b7 35 IMU.calibrateMag(0);
hbindra3 5:507618748cb4 36
hbindra3 5:507618748cb4 37 // initial_direction = getDirection();
apcastelein 3:b65b394375b7 38 }
hbindra3 5:507618748cb4 39 */
apcastelein 4:104acd180bb1 40 //verify color sensor and initialize
apcastelein 4:104acd180bb1 41 void initColorSensor(){
apcastelein 4:104acd180bb1 42 pc.baud(9600);
apcastelein 4:104acd180bb1 43
apcastelein 4:104acd180bb1 44 // Connect to the Color sensor and verify
apcastelein 4:104acd180bb1 45 color_sensor.frequency(200000);
apcastelein 4:104acd180bb1 46
apcastelein 4:104acd180bb1 47 char id_regval[1] = {146};
apcastelein 4:104acd180bb1 48 char data[1] = {0};
apcastelein 4:104acd180bb1 49 color_sensor.write(color_addr,id_regval,1, true);
apcastelein 4:104acd180bb1 50 color_sensor.read(color_addr,data,1,false);
apcastelein 4:104acd180bb1 51
apcastelein 4:104acd180bb1 52 // Initialize color sensor
apcastelein 4:104acd180bb1 53
apcastelein 4:104acd180bb1 54 char timing_register[2] = {129,0};
apcastelein 4:104acd180bb1 55 color_sensor.write(color_addr,timing_register,2,false);
apcastelein 4:104acd180bb1 56
apcastelein 4:104acd180bb1 57 char control_register[2] = {143,0};
apcastelein 4:104acd180bb1 58 color_sensor.write(color_addr,control_register,2,false);
apcastelein 4:104acd180bb1 59
apcastelein 4:104acd180bb1 60 char enable_register[2] = {128,3};
apcastelein 4:104acd180bb1 61 color_sensor.write(color_addr,enable_register,2,false);
apcastelein 4:104acd180bb1 62 }
apcastelein 4:104acd180bb1 63
apcastelein 3:b65b394375b7 64 //initialization for all components
apcastelein 3:b65b394375b7 65 void init(){
hbindra3 5:507618748cb4 66 //initIMU();
apcastelein 4:104acd180bb1 67 initColorSensor();
apcastelein 3:b65b394375b7 68 }
apcastelein 0:67dc92eb6d5f 69
apcastelein 0:67dc92eb6d5f 70 // Sends a request to the server and waits for a path response
apcastelein 0:67dc92eb6d5f 71 char* getPathFromServer(){
apcastelein 0:67dc92eb6d5f 72 //use wifi module here
apcastelein 0:67dc92eb6d5f 73 //https://developer.mbed.org/users/electromotivated/notebook/wifi-pid-redbot-robot-webserver/
apcastelein 0:67dc92eb6d5f 74 char* path = "";
apcastelein 0:67dc92eb6d5f 75 return path;
apcastelein 0:67dc92eb6d5f 76 }
apcastelein 0:67dc92eb6d5f 77
apcastelein 1:849b06ea572d 78 //gets the orientation from imu
apcastelein 2:c91ce1f091f5 79 float getDirection(){
apcastelein 3:b65b394375b7 80 float sum = 0;
apcastelein 3:b65b394375b7 81 int target_samples = 100;
apcastelein 3:b65b394375b7 82 int num_samples = 0;
apcastelein 3:b65b394375b7 83 while(num_samples++ < target_samples){
hbindra3 5:507618748cb4 84
apcastelein 3:b65b394375b7 85 while(!IMU.magAvailable(X_AXIS));
apcastelein 3:b65b394375b7 86 IMU.readMag();
apcastelein 3:b65b394375b7 87 float my = IMU.calcMag(IMU.my);
apcastelein 3:b65b394375b7 88 float mx = IMU.calcMag(IMU.mx);
apcastelein 3:b65b394375b7 89 float dir;
apcastelein 3:b65b394375b7 90 if (my == 0.0)
apcastelein 3:b65b394375b7 91 dir = (mx < 0.0) ? 180.0 : 0.0;
apcastelein 3:b65b394375b7 92 else
apcastelein 3:b65b394375b7 93 dir = atan2(mx, my)*360.0/(2.0*PI);
apcastelein 3:b65b394375b7 94 sum += dir;
apcastelein 3:b65b394375b7 95 }
hbindra3 5:507618748cb4 96
apcastelein 3:b65b394375b7 97 return sum / target_samples;
apcastelein 2:c91ce1f091f5 98 }
apcastelein 1:849b06ea572d 99
hbindra3 5:507618748cb4 100 float getRelativeDirection(){
hbindra3 5:507618748cb4 101 return getDirection() - initial_direction;
hbindra3 5:507618748cb4 102 }
hbindra3 5:507618748cb4 103
apcastelein 4:104acd180bb1 104 //returns true when color is entered (subsequently returns false until color is exited and entered again)
apcastelein 4:104acd180bb1 105 bool colorEntered(){
hbindra3 5:507618748cb4 106 char red_reg[1] = {150};
hbindra3 5:507618748cb4 107 char red_data[2] = {0,0};
hbindra3 5:507618748cb4 108 color_sensor.write(color_addr,red_reg,1, true);
hbindra3 5:507618748cb4 109 color_sensor.read(color_addr,red_data,2, false);
apcastelein 4:104acd180bb1 110
hbindra3 5:507618748cb4 111 int red_value = ((int)red_data[1] << 8) | red_data[0];
apcastelein 4:104acd180bb1 112
hbindra3 5:507618748cb4 113 bool is_red = red_value > color_thresh;
hbindra3 5:507618748cb4 114 bool color_triggered = !on_color && is_red;
hbindra3 5:507618748cb4 115 on_color = is_red;
hbindra3 5:507618748cb4 116 //pc.printf("%d\r\n",red_value);
apcastelein 4:104acd180bb1 117 return color_triggered;
apcastelein 4:104acd180bb1 118 }
apcastelein 4:104acd180bb1 119
apcastelein 0:67dc92eb6d5f 120 //technologies for navigation may include
apcastelein 0:67dc92eb6d5f 121 // - Line following https://www.sparkfun.com/products/11769
apcastelein 0:67dc92eb6d5f 122 // - color sensor (indicates when end of movement is reached)
apcastelein 0:67dc92eb6d5f 123 void moveForward(){}
hbindra3 5:507618748cb4 124 void rotateLeft(){
hbindra3 5:507618748cb4 125 left.speed(0.3);
hbindra3 5:507618748cb4 126 right.speed(0.3);
hbindra3 5:507618748cb4 127 while(!colorEntered()) wait(.01);
hbindra3 5:507618748cb4 128 }
apcastelein 0:67dc92eb6d5f 129 void rotateRight(){}
apcastelein 0:67dc92eb6d5f 130
apcastelein 0:67dc92eb6d5f 131 //arem technology is yet to be determined, will probably involve adjusting a motor
apcastelein 0:67dc92eb6d5f 132 void armUp(){}
apcastelein 0:67dc92eb6d5f 133 void armDown(){}
apcastelein 0:67dc92eb6d5f 134
apcastelein 0:67dc92eb6d5f 135 // Loops through path commands and executes each one sequentially
apcastelein 0:67dc92eb6d5f 136 void executePath(char* path){
apcastelein 0:67dc92eb6d5f 137 int index = 0;
apcastelein 0:67dc92eb6d5f 138 while(path[index] != '\0'){
apcastelein 0:67dc92eb6d5f 139 switch(path[index]){
apcastelein 0:67dc92eb6d5f 140 case 'F': moveForward(); break;
apcastelein 0:67dc92eb6d5f 141 case 'L': rotateLeft(); break;
apcastelein 0:67dc92eb6d5f 142 case 'R': rotateRight(); break;
apcastelein 0:67dc92eb6d5f 143 case 'U': armUp(); break;
apcastelein 3:b65b394375b7 144 case 'D': armDown(); break;
apcastelein 0:67dc92eb6d5f 145 }
apcastelein 0:67dc92eb6d5f 146 }
apcastelein 0:67dc92eb6d5f 147 }
apcastelein 0:67dc92eb6d5f 148
apcastelein 3:b65b394375b7 149 //main code
apcastelein 3:b65b394375b7 150 //- repeatedly listens for paths and then executes them
apcastelein 0:67dc92eb6d5f 151 int main() {
apcastelein 3:b65b394375b7 152 init();
apcastelein 0:67dc92eb6d5f 153 while(true){
hbindra3 5:507618748cb4 154 while(!colorEntered()) wait(.1);
hbindra3 5:507618748cb4 155 green = 1;
hbindra3 5:507618748cb4 156 wait(5);
hbindra3 5:507618748cb4 157 green = 0;
hbindra3 5:507618748cb4 158 rotateLeft();
hbindra3 5:507618748cb4 159 left.speed(0);
hbindra3 5:507618748cb4 160 right.speed(0);
hbindra3 5:507618748cb4 161 wait(1);
hbindra3 5:507618748cb4 162 /*green = colorEntered();
hbindra3 5:507618748cb4 163 pc.printf("%f\r\n",getDirection());
hbindra3 5:507618748cb4 164 pc.printf("%f\r\n",getRelativeDirection());
hbindra3 5:507618748cb4 165 char* path = getPathFromServer(); */
hbindra3 5:507618748cb4 166 //executePath(path);
hbindra3 5:507618748cb4 167 //wait(.1);
apcastelein 0:67dc92eb6d5f 168 }
apcastelein 0:67dc92eb6d5f 169 }