main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "WS2812.h"
00004 #include "PixelArray.h" 
00005  
00006 #define WS2812_BUF 10
00007 #define NUM_COLORS 1
00008 #define NUM_LEDS_PER_COLOR 10
00009 PixelArray px(WS2812_BUF);
00010 
00011 // See the program page for information on the timing numbers
00012 // The given numbers are for the K64F
00013 WS2812 ws(p9, WS2812_BUF, 5, 10, 10, 15);
00014 
00015 PwmOut Servo_Y(p22);
00016 PwmOut Servo_X(p21);
00017 PwmOut Servo_Z(p23);
00018 
00019 Serial pc(USBTX, USBRX); // tx, rx
00020 
00021  
00022 float Servo_z_smooth;
00023 
00024 void Servo_Z_Thread(void const *args) {
00025     float Sevro_Pos;
00026     float Sevro_Pos_New = 0.1;
00027     float Sevro_Pos_Old = 0.01;
00028     
00029     while (true) {
00030         
00031         Sevro_Pos_New = Servo_z_smooth;
00032         
00033         if(Sevro_Pos_Old <= Sevro_Pos_New)
00034             {
00035             for(float i=Sevro_Pos_Old*10000; i<=Sevro_Pos_New*10000; i=i+25)
00036                 {
00037                 Sevro_Pos = i/10000;
00038                 Servo_Z = Sevro_Pos;
00039                 pc.printf("Servo_Z1: %1.6f %1.6f %1.6f\n\r ",Sevro_Pos, i,Sevro_Pos_Old);
00040                 //Thread::wait(50);
00041                 }
00042             }
00043         else
00044             {
00045             for(float i=Sevro_Pos_Old*10000; i>=Sevro_Pos_New*10000; i=i-25)
00046                 {
00047                 Sevro_Pos = i/10000;
00048                 Servo_Z = Sevro_Pos;
00049                 pc.printf("Servo_Z2: %1.6f \n\r",Sevro_Pos);
00050                 //Thread::wait(50);
00051                 }            
00052             }
00053         Sevro_Pos_Old = Sevro_Pos_New;
00054         Thread::wait(500);
00055     }    
00056 }
00057  
00058  
00059  
00060 void LED_All_Colour(int red , int green ,int blue , int bright)
00061     {
00062     ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
00063     
00064     int colorbuf[NUM_COLORS] = {0x000000};
00065     //int colorbuf[NUM_COLORS] = {0x000000,0x00f0ff,0x00ff00,0x00ffff,0xffffff,0x00ff00,0x00ffff,0x0000ff,0xff00ff};
00066     
00067     red <<= 16;
00068     green <<= 8;
00069     
00070     colorbuf[0] = red + green + blue;
00071     
00072     pc.printf("Colour: %6x red:%6x  Green:%6x  Blue:%6x Bright:%x \n\r",colorbuf[0], red , green , blue, bright);
00073     // for each of the colours (j) write out 10 of them
00074     // the pixels are written at the colour*10, plus the colour position
00075     // all modulus 60 so it wraps around
00076     for (int i = 0; i < WS2812_BUF; i++) {
00077         px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
00078     }
00079     
00080     // now all the colours are computed, add a fade effect using intensity scaling
00081     // compute and write the II value for each pixel
00082     for (int j=0; j<WS2812_BUF; j++) {
00083         // px.SetI(pixel position, II value)
00084         //px.SetI(j%WS2812_BUF, 0xff+(0xf*(j%NUM_LEDS_PER_COLOR)));     //full brightness
00085         px.SetI(j%WS2812_BUF, bright +(bright*(j%NUM_LEDS_PER_COLOR)));        //control brightness    
00086     }
00087     //set the colour of the LEDs
00088     for (int z=WS2812_BUF; z >= 0 ; z--) {
00089         ws.write_offsets(px.getBuf(),z,z,z);
00090         
00091 
00092         }    
00093 }
00094 
00095 
00096 void LED_Colour_Pos(int position, int red , int green ,int blue , int bright)
00097     {
00098     ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
00099     
00100     int colorbuf = 0x000000;
00101     
00102     red <<= 16;
00103     green <<= 8;
00104     
00105     colorbuf = red + green + blue;
00106     
00107     pc.printf("Colour: %6x red:%6x  Green:%6x  Blue:%6x Bright:%x \n\r",colorbuf, red , green , blue, bright);
00108     // for each of the colours (j) write out 10 of them
00109     // the pixels are written at the colour*10, plus the colour position
00110     // all modulus 60 so it wraps around
00111     px.Set(position, colorbuf);
00112     
00113     // now all the colours are computed, add a fade effect using intensity scaling
00114     // compute and write the II value for each pixel
00115     //px.SetI(pixel position, II value)
00116     px.SetI(position, bright);        //control brightness    
00117 }
00118 
00119 void LED_Colour_Pos_Set()
00120 {
00121     //set the colour of the LEDs
00122     for (int z=WS2812_BUF-1; z >= 0 ; z--) 
00123     {
00124         ws.write_offsets(px.getBuf(),z,z,z);
00125     } 
00126 }
00127 
00128 
00129 void Smooth_Fade(int h , int speed)
00130 {
00131    for(int j = 0; j<=h;j++)
00132     {
00133         for(int i = 0;i<=255;i=i+speed)
00134         {
00135             LED_All_Colour(i, 0 ,255-i , 255);
00136             //wait(0.01);
00137         }    
00138         for(int i = 0;i<=255;i=i+speed)
00139         {
00140             LED_All_Colour(255-i, i ,0 , 255);
00141             //wait(0.01);
00142         } 
00143         for(int i = 0;i<=255;i=i+speed)
00144         {
00145             LED_All_Colour(0, 255-i ,i , 255);
00146             //wait(0.01);
00147         } 
00148     }  
00149 }
00150 
00151 
00152 void Red_Blue_Flash(int h)
00153 {
00154    for(int i = 0;i<=h;i++)
00155         {
00156             
00157             LED_All_Colour(255, 0 ,0 , 50);
00158             wait(0.02);
00159             LED_All_Colour(255, 0 ,0 , 255);
00160             wait(0.02);
00161             LED_All_Colour(0, 0 , 255 , 50);
00162             wait(0.02);            
00163             LED_All_Colour(0, 0 , 255 , 255);
00164             wait(0.02);
00165         }   
00166 }
00167 
00168  
00169 int main() {
00170     Thread thread_Servo_Z(Servo_Z_Thread );
00171                 
00172     
00173     while (true) {
00174        /////////////////////////////////////////////////////////////////////////////////////
00175        ////////////////////////////////////   X   //////////////////////////////////////////
00176        /////////////////////////////////////////////////////////////////////////////////////
00177         Servo_Y = 0.05;
00178         Servo_X = 0.06;
00179         Servo_z_smooth = 0.01;
00180         LED_Colour_Pos(0,255,0,0,128);
00181         LED_Colour_Pos(1,0,255,0,128);
00182         LED_Colour_Pos(2,0,0,255,128);
00183         LED_Colour_Pos(3,255,255,0,128);  
00184         LED_Colour_Pos(4,255,0,255,128); 
00185         LED_Colour_Pos(5,128,255,0,128);
00186         LED_Colour_Pos(6,128,50,0,255);
00187         LED_Colour_Pos(7,0,255,128,128);
00188         LED_Colour_Pos(8,128,0,128,255);  
00189         LED_Colour_Pos(9,128,128,255,128); 
00190         LED_Colour_Pos_Set();        
00191         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4 \n\r",Servo_X, Servo_Y, Servo_z_smooth);
00192         Thread::wait(1000);
00193         Servo_Y = 0.05;
00194         Servo_X = 0.06;
00195         Servo_z_smooth = 0.05;
00196         LED_Colour_Pos(0,128,255,0,128);
00197         LED_Colour_Pos(1,128,50,0,255);
00198         LED_Colour_Pos(2,0,255,128,128);
00199         LED_Colour_Pos(3,128,0,128,255);  
00200         LED_Colour_Pos(4,128,128,255,128); 
00201         LED_Colour_Pos(5,255,255,0,255);
00202         LED_Colour_Pos(6,128,50,127,255);
00203         LED_Colour_Pos(7,255,255,0,128);
00204         LED_Colour_Pos(8,128,0,128,255);  
00205         LED_Colour_Pos(9,0,128,255,128);         
00206         LED_Colour_Pos_Set();         
00207         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00208         Thread::wait(500);
00209         Servo_Y = 0.05;
00210         Servo_X = 0.08;
00211         Servo_z_smooth = 0.08;
00212         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00213         Thread::wait(500); 
00214         Servo_Y = 0.06;
00215         Servo_X = 0.05;
00216         Servo_z_smooth = 0.1;
00217         LED_All_Colour(255, 0 , 0 , 255);
00218         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00219         Thread::wait(500); 
00220         Servo_Y = 0.05;
00221         Servo_X = 0.05;
00222         Servo_z_smooth = 0.05;
00223         LED_All_Colour(255, 255 , 0 , 255);        
00224         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00225         Thread::wait(500); 
00226         Servo_Y = 0.05;
00227         Servo_X = 0.09;
00228         Servo_z_smooth = 0.05;
00229         LED_All_Colour(0, 0 , 255 , 255);        
00230         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00231         Thread::wait(500); 
00232         Servo_Y = 0.05;
00233         Servo_X = 0.05;
00234         Servo_z_smooth = 0.05;
00235         LED_All_Colour(0, 255 , 0 , 255);        
00236         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00237         Thread::wait(500);     
00238         Servo_Y = 0.05;
00239         Servo_X = 0.09;
00240         Servo_z_smooth = 0.05;
00241         LED_Colour_Pos(0,255,0,0,128);
00242         LED_Colour_Pos(1,0,255,0,128);
00243         LED_Colour_Pos(2,0,0,255,128);
00244         LED_Colour_Pos(3,255,255,0,128);  
00245         LED_Colour_Pos(4,255,0,255,128); 
00246         LED_Colour_Pos(5,128,255,0,128);
00247         LED_Colour_Pos(6,128,50,0,255);
00248         LED_Colour_Pos(7,0,255,128,128);
00249         LED_Colour_Pos(8,128,0,128,255);  
00250         LED_Colour_Pos(9,128,128,255,128); 
00251         LED_Colour_Pos_Set(); 
00252         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00253         Thread::wait(500); 
00254         Servo_Y = 0.05;
00255         Servo_X = 0.05;
00256         Servo_z_smooth = 0.05;
00257         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00258         Thread::wait(500);  
00259         Servo_Y = 0.05;
00260         Servo_X = 0.08;
00261         Servo_z_smooth = 0.05;
00262         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00263         Thread::wait(500); 
00264         
00265        /////////////////////////////////////////////////////////////////////////////////////
00266        ////////////////////////////////////   y   //////////////////////////////////////////
00267        /////////////////////////////////////////////////////////////////////////////////////
00268         Servo_Y = 0.055;
00269         Servo_X = 0.08;
00270         Servo_z_smooth = 0.05;
00271         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00272         Thread::wait(500);   
00273         Servo_Y = 0.08;
00274         Servo_X = 0.075;
00275         Servo_z_smooth = 0.05;
00276         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00277         Thread::wait(500); 
00278         Servo_Y = 0.055;
00279         Servo_X = 0.075;
00280         Servo_z_smooth = 0.05;
00281         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00282         Thread::wait(500); 
00283         Servo_Y = 0.065;
00284         Servo_X = 0.075;
00285         Servo_z_smooth = 0.05;
00286         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00287         Thread::wait(1000); 
00288         Servo_Y = 0.055;
00289         Servo_X = 0.075;
00290         Servo_z_smooth = 0.05;
00291         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00292         Thread::wait(500);   
00293         Servo_Y = 0.065;
00294         Servo_X = 0.075;
00295         Servo_z_smooth = 0.05;
00296         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00297         Thread::wait(500); 
00298         Servo_Y = 0.055;
00299         Servo_X = 0.075;
00300         Servo_z_smooth = 0.05;
00301         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00302         Thread::wait(500);  
00303         Servo_Y = 0.065;
00304         Servo_X = 0.075;
00305         Servo_z_smooth = 0.05;
00306         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00307         Thread::wait(500); 
00308         Servo_Y = 0.06;
00309         Servo_X = 0.075;
00310         Servo_z_smooth = 0.09;
00311         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00312         Thread::wait(750);                             
00313         
00314         /////////////////////////////////////////////////////////////////////////////////////
00315         ////////////////////////////////////   Z   //////////////////////////////////////////
00316         /////////////////////////////////////////////////////////////////////////////////////
00317         Servo_Y = 0.06;
00318         Servo_X = 0.075;
00319         Servo_z_smooth = 0.08;
00320         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00321         Thread::wait(500);  
00322         Servo_Y = 0.06;
00323         Servo_X = 0.075;
00324         Servo_z_smooth = 0.1;
00325         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00326         Thread::wait(500);                             
00327         Servo_Y = 0.06;
00328         Servo_X = 0.075;
00329         Servo_z_smooth = 0.07;
00330         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00331         Thread::wait(500);
00332         Servo_Y = 0.06;
00333         Servo_X = 0.075;
00334         Servo_z_smooth = 0.11;
00335         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00336         Thread::wait(750); 
00337         Servo_Y = 0.06;
00338         Servo_X = 0.075;
00339         Servo_z_smooth = 0.07;
00340         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00341         Thread::wait(750);  
00342         Servo_Y = 0.06;
00343         Servo_X = 0.075;
00344         Servo_z_smooth = 0.11;
00345         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00346         Thread::wait(750);                             
00347         Servo_Y = 0.06;
00348         Servo_X = 0.075;
00349         Servo_z_smooth = 0.07;
00350         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00351         Thread::wait(750);
00352         Servo_Y = 0.06;
00353         Servo_X = 0.075;
00354         Servo_z_smooth = 0.11;
00355         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00356         Thread::wait(750);                            
00357         Servo_Y = 0.06;
00358         Servo_X = 0.075;
00359         Servo_z_smooth = 0.07;
00360         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00361         Thread::wait(1500);                          
00362         Servo_Y = 0.06;
00363         Servo_X = 0.75;
00364         Servo_z_smooth = 0.03;
00365         printf("Servo X: %f1.4 Servo Y: %f1.4 Servo Z: %f1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00366         Thread::wait(1500);                    
00367     }
00368 }