main.cpp

00001 #include "DotStar.h"
00002 #include "rtos.h"
00003 
00004 enum Init {brightness = 4, i2cHz = 400000, baud = 9600};
00005 enum Register{ WHO_AM_I = 0x75, PWR_MGMT_1 = 0x6b, GYRO_CONFIG = 0x1b, CONFIG = 0x1a, GYRO_OFFSET = 0x13, GYRO = 0x43 };
00006 
00007 Serial  dev(USBTX,USBRX);
00008 DigitalOut cts(PA_6, 0);
00009 SPI spiOne(PB_5, PB_4, PB_3);
00010 I2C accel(PA_10,PA_9);
00011 const int nLedsA = 65;
00012 const int accRead = 0x68 << 1;
00013 const int accWrite = accRead | 0x01;
00014 
00015 struct XYZAccel {
00016     int16_t x,y,z;
00017 };
00018 
00019 XYZAccel getXYZ() {
00020     XYZAccel result;
00021     char reg[1] = {Register::GYRO};
00022     char data[6];
00023     int ret = accel.write(accWrite,reg,1, true);
00024     ret = accel.read(accRead,data,6);
00025     result.x  = (((int16_t)data[0]) << 8) | data[1];
00026     result.y  = (((int16_t)data[2]) << 8) | data[3];
00027     result.z  = (((int16_t)data[4]) << 8) | data[5];
00028     return result;
00029 };
00030 
00031 int main() {
00032     DotStar dotStarA(&spiOne, nLedsA);
00033     Thread::wait(1000);
00034     accel.frequency(Init::i2cHz);
00035     dev.baud(Init::baud);
00036     /* for (int i = 1; i < DotStar::brightest; i += 4) { */
00037     /*     dotStarA.set_brightness(i); */
00038     /* } */
00039     dotStarA.set_brightness(DotStar::half);
00040     int ret = 0;
00041     char reg[1];
00042     char data[7] = {};
00043     reg[0] = Register::WHO_AM_I;
00044     if(accel.write(accWrite,reg,1, true) != 0){
00045         dev.printf("writing RA failed\n");
00046     };
00047     ret = accel.read(accRead,data,1);
00048     if (ret != 0) dev.printf("reading RA failed\n");
00049     dev.printf("%i\n",data[0]);
00050 
00051     reg[0] = PWR_MGMT_1;
00052     ret = accel.write(accWrite,reg,1, true);
00053     if (ret != 0) dev.printf("writing RA failed\n");
00054     ret = accel.read(accRead,data,1);
00055     if (ret != 0) dev.printf("reading RA failed\n");
00056     dev.printf("%i\n",data[0]);
00057     data[0] &= ~(1<< 6);
00058     dev.printf("%i\n",data[0]);
00059     data[0] |= 1;
00060     dev.printf("%i\n",data[0]);
00061     data[1] = data[0];
00062     data[0]= reg[0];
00063     ret = accel.write(accWrite,data,2);
00064     if (ret != 0) dev.printf("writing data failed\n");
00065 
00066     ret = accel.write(accWrite,reg,1, true);
00067     if (ret != 0) dev.printf("writing RA failed\n");
00068     ret = accel.read(accRead,data,1);
00069     if (ret != 0) dev.printf("reading RA failed\n");
00070     dev.printf("%i\n",data[0]);
00071 
00072     reg[0] = Register::GYRO_CONFIG;
00073     ret = accel.write(accWrite,reg,1, true);
00074     if (ret != 0) dev.printf("writing RA failed\n");
00075     ret = accel.read(accRead,data,1);
00076     if (ret != 0) dev.printf("reading RA failed\n");
00077     dev.printf("%i\n",data[0]);
00078     data[0] |= 1<< 4;
00079     data[0] |= 1<< 3;
00080     data[0] &= ~1;
00081     data[0] &= ~(1<<1);
00082     dev.printf("%i\n",data[0]);
00083     data[1] = data[0];
00084     data[0]= reg[0];
00085     ret = accel.write(accWrite,data,2);
00086     if (ret != 0) dev.printf("writing data failed\n");
00087 
00088     reg[0] = Register::CONFIG;
00089     ret = accel.write(accWrite,reg,1, true);
00090     if (ret != 0) dev.printf("writing RA failed\n");
00091     ret = accel.read(accRead,data,1);
00092     if (ret != 0) dev.printf("reading RA failed\n");
00093     dev.printf("%i\n",data[0]);
00094     data[0] &= ~(1<< 2);
00095     data[0] &= ~1;
00096     data[0] &= ~(1<< 1);
00097     dev.printf("%i\n",data[0]);
00098     data[1] = data[0];
00099     data[0]= reg[0];
00100     ret = accel.write(accWrite,data,2);
00101     if (ret != 0) dev.printf("writing data failed\n");
00102 
00103     reg[0] = Register::GYRO_OFFSET;
00104     ret = accel.write(accWrite,reg,1, true);
00105     if (ret != 0) dev.printf("writing RA failed\n");
00106     ret = accel.read(accRead,data,6);
00107     if (ret != 0) dev.printf("reading RA failed\n");
00108     for (int i = 6; i < 1; --i) {
00109         data[i] = data[i-1];
00110     }
00111     data[0] = reg[0];
00112     data[1] = -1;
00113     data[3] = -1;
00114     data[5] = -1;
00115     data[2] = -1;
00116     data[4] = -8;
00117     data[6] = -18;
00118     ret = accel.write(accWrite,data,7);
00119     if (ret != 0) dev.printf("writing data failed\n");
00120     Thread::wait(500);
00121 
00122     /* for(int i = 0; i < 5; ++i){ */
00123     /* reg[0] = 0x43; */
00124     /* ret = accel.write(accWrite,reg,1, true); */
00125     /* if (ret != 0) dev.printf("writing RA failed\n"); */
00126     /* ret = accel.read(accRead,data,6); */
00127     /* if (ret != 0) dev.printf("reading RA failed\n"); */
00128     /* int16_t xAcc = (((int16_t)data[0]) << 8) | (int16_t)(data[1]); */
00129     /* int16_t yAcc = (((int16_t)data[2]) << 8) | (int16_t)(data[3]); */
00130     /* int16_t zAcc = (((int16_t)data[4]) << 8) | (int16_t)(data[5]); */
00131     /* dev.printf("x:%i  ",(xAcc)); */
00132     /* dev.printf("y:%i  ",(yAcc)); */
00133     /* dev.printf("z:%i\n",(zAcc)); */
00134     /* Thread::wait(500); */
00135     /* } */
00136 
00137     /* Thread::wait(100); */
00138     /* uint8_t dataInt = accel.read(0); */
00139     /* accel.stop(); */
00140     /* reg[0] = 0x75; */
00141     /* reg[0] = 6; */
00142     /* ret = accel.write(i2caddr,reg, 2); */
00143     /* ret = accel.read(i2caddr,data,1); */
00144     /* reg[0] = 0x1c; */
00145     /* reg[1] = 0; */
00146     /* ret = accel.write(i2caddr,reg,2); */
00147     /* dotStarA.set_brightness(ret*10); */
00148     /* reg[0] = 0x3b; */
00149     /* ret = accel.write(i2caddr,reg,1); */
00150     /* dotStarA.set_brightness(ret*10); */
00151     /* ret = accel.read(i2caddr,data,6); */
00152     /* dotStarA.set_brightness(ret*10); */
00153     /* struct Dirs{float x, y, z;}; */
00154     /* Dirs a; */
00155     /* a.x = short( (((short)data[1]) << 8) | data[0] ); */
00156     /* a.y = short( (((short)data[3]) << 8) | data[2] ); */
00157     /* a.z = short( (((short)data[5]) << 8) | data[4] ); */
00158     /* if(data[0] > -1 && data[0] < 4) dev.putc('y'); */
00159     /* dev.printf("%f",(a.x)); */
00160     /* dev.printf("%f",(a.y)); */
00161     /* dev.printf("%f",(a.z)); */
00162     dotStarA.set_brightness(DotStar::half);
00163     for (;;) {
00164         XYZAccel xyzData = getXYZ();
00165         RedGreenBlue rgb;
00166         rgb.red = abs(xyzData.x)>>6;
00167         rgb.green = abs(xyzData.y)>>6;
00168         rgb.blue = abs(xyzData.z)>>6;
00169         if(rgb.red < rgb.green && rgb.red < rgb.blue) rgb.red = 0;
00170         else if(rgb.blue < rgb.green && rgb.blue < rgb.red) rgb.blue = 0;
00171         else rgb.green = 0;
00172         dotStarA.set_rgb(rgb);
00173     }
00174 
00175 }