main.cpp

00001 #include "mbed.h"
00002 //#include "stdio.h"
00003 #include "QEI.h"
00004 #include "string"
00005 #include "Arial12x12.h"
00006 #include "Arial24x23.h"
00007 #include "Arial28x28.h"
00008 #include "font_big.h"
00009 #include "bigFo.h"
00010 #include "ILI9341.h"
00011 #include "Arial43x48_numb.h"
00012 #include "DebounceIn.h"
00013 #include "CPU_Usage.h"
00014 
00015 
00016 
00017 #define PI (3.141592653589793)
00018 
00019 #define encoderRATE  100
00020 #define lcdRATE  100
00021 #define stepRATE  15
00022 
00023 #define mmRatio  4450
00024 
00025 #define stAngle  0
00026 #define stFeed 1
00027 #define stThread 2
00028 
00029 #define editPos 0
00030 #define editMult 1
00031 #define editFeed 2
00032 
00033 Timer t;
00034 
00035 DigitalOut myled1(LED1);
00036 
00037 Serial pc(USBTX, USBRX);
00038 
00039 
00040 Thread threadEnc(osPriorityNormal);
00041 Thread threadStep(osPriorityAboveNormal);
00042 
00043 DebounceIn  wheelPanButton(PC_12);
00044 
00045 DebounceIn  pos1Button(PB_12);
00046 DebounceIn  pos2Button(PA_12);
00047 DebounceIn  selectButton(PA_11);
00048 
00049 InterruptIn  switch1Button(PC_8);
00050 InterruptIn  switch2Button(PC_6);
00051 InterruptIn  switch3Button(PC_5);
00052 //PC_6 midle feed rubilnik pin
00053 
00054 
00055 DigitalOut stepPin(PB_1);
00056 DigitalOut dirPin(PB_2);
00057 
00058 
00059 //Use X4 encoding.
00060 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
00061 //Use X2 encoding by default.
00062 //QEI wheel (p29, p30, NC, 624);
00063 QEI wheel (PB_13, PB_14, PB_15, 4000, QEI::X4_ENCODING);
00064 
00065 QEI wheelPan (PC_11, PC_10, NC, 20, QEI::X4_ENCODING);
00066 
00067 PinName buspins[8]={D8,D9,D2,D3,D4,D5,D6,D7};
00068 ILI9341 myLCD(BUS_8, buspins, A3, A4, A2, A1, A0,"myLCD"); // Parallel Bus 8bit, buspins array, CS, reset, RS, WR, RD
00069 
00070 
00071 unsigned short backgroundcolor=Blue;
00072 unsigned short foregroundcolor=White;
00073 
00074 
00075 
00076 int state=1, prevState=-1;
00077 
00078 volatile int dir,z=1, k=7, edit=0;
00079 
00080 volatile float stPos=0, stPrevPos=1, stTarget=0;
00081 volatile float stepMult =1, prevStepMult=0, pos1, pos2, prevPos1, prevPos2;
00082 
00083 float multMass[2]={0.1,1};
00084 int feedMass[10]={30,60,120,180,240,300,450,600,900,1200};
00085 
00086 volatile int Pulses     = 0, panPulses=0, feedRate = 450, delta;
00087 volatile int PrevPulses = 0;
00088 volatile int rev = 0;
00089 volatile int prevRev = 0;
00090 volatile int panAngle=0, prevPanAngle=0;
00091 volatile float stepPeriod = stepRATE;
00092 volatile float Angle = 0;
00093 float prevAngle=0;
00094 bool wheelBut, prevWheelBut, startUp=true, showStopPos=1;
00095 
00096     
00097 void printWheelPan(){
00098     myLCD.set_font((unsigned char*) SCProSB31x55);
00099     myLCD.locate(50,50);
00100     myLCD.printf("%.1f ", Angle);
00101     myLCD.set_font((unsigned char*) Arial43x48_numb);
00102     myLCD.locate(50,50);
00103     myLCD.printf("%.1f ", Angle);
00104     }
00105     
00106 void printWheelBut(){
00107     myLCD.set_font((unsigned char*) Arial24x23);
00108     myLCD.locate(150,50);
00109     myLCD.printf("But %.1i ", wheelBut);
00110     }
00111     
00112 void printMult(){
00113     myLCD.set_font((unsigned char*) Arial24x23);
00114     myLCD.locate(90,215);
00115     myLCD.printf("%.1f", stepMult);
00116     }
00117     
00118 void printFeed(){
00119     myLCD.set_font((unsigned char*) Arial24x23);
00120     myLCD.locate(250,215);
00121     myLCD.printf("%i", feedRate);
00122     }
00123     
00124 void printTrPos(){
00125     myLCD.set_font((unsigned char*) Arial24x23);
00126     myLCD.locate(90,185);
00127     myLCD.printf("%.1f", pos1/mmRatio);
00128     }
00129     
00130 void printPos2(){
00131     myLCD.set_font((unsigned char*) Arial24x23);
00132     myLCD.locate(250,185);
00133     myLCD.printf("%.1f", pos2/mmRatio);
00134     }
00135     
00136 void printStPos(){
00137     myLCD.foreground(Green); 
00138     myLCD.set_font((unsigned char*) Arial12x12);
00139     myLCD.locate(10,95);
00140     float val=stTarget/mmRatio;
00141     myLCD.printf("%.1f ", val);
00142     myLCD.foreground(White); 
00143     myLCD.set_font((unsigned char*) SCProSB31x55);
00144     myLCD.locate(60,70);
00145     val=stPos/mmRatio;
00146     myLCD.printf("%.1f ", val);
00147     }
00148 
00149 
00150 void printStAngle(){
00151     myLCD.cls();   
00152     myLCD.set_orientation(1);   
00153     myLCD.background(Blue);    // set background to black
00154     myLCD.foreground(White);    // set chars to white
00155     myLCD.rect(0,0,320,240, Blue);
00156     myLCD.set_font((unsigned char*) Arial28x28);
00157     myLCD.locate(100,10);
00158     myLCD.printf("ANGLE mode");
00159     myLCD.set_font((unsigned char*) Arial24x23);
00160     myLCD.locate(200,300);
00161     myLCD.printf("o");
00162     }
00163   //PRINT FEED MODE  
00164 void printStFeed(){
00165     myLCD.cls();   
00166     myLCD.set_orientation(1);   
00167     myLCD.background(Blue);    // set background to black
00168     myLCD.foreground(White);    // set chars to white
00169     myLCD.rect(0,0,320,240, Blue);
00170     myLCD.set_font((unsigned char*) Arial28x28);
00171     myLCD.locate(75,10);
00172     myLCD.printf(" FEED mode");
00173     myLCD.background(Black);
00174     myLCD.set_font((unsigned char*) Arial24x23);
00175     myLCD.locate(5,70);
00176     myLCD.printf("pos ");
00177     myLCD.rect(0,180,160,210, Blue);
00178     myLCD.rect(160,180,320,210, Blue);
00179     myLCD.rect(160,210,320,240, Blue);
00180     myLCD.rect(0,210,160,240, Blue);
00181     myLCD.set_font((unsigned char*) Arial24x23);
00182     myLCD.locate(5,215);
00183     myLCD.printf("Mult:");
00184     myLCD.locate(165,215);
00185     myLCD.printf("Feed:");
00186     myLCD.locate(5,185);
00187     myLCD.printf("Pos1:");
00188     //myLCD.locate(165,185);
00189     //myLCD.printf("Pos2:");
00190     printFeed();
00191     }
00192     
00193 void pitnTmpVar(float var){
00194     myLCD.set_font((unsigned char*) Arial12x12);
00195     myLCD.locate(5,5);
00196     myLCD.printf("%.1f ", var);
00197     }
00198     
00199 void encoderPan_thread() {
00200     while(1){
00201 
00202         wheelBut=wheelPanButton.read();
00203         if(!wheelBut){
00204             int prevPanPulses=panPulses;
00205             panPulses=wheelPan.getPulses()/4;
00206             delta = (prevPanPulses - panPulses);
00207             if(edit==0){
00208                 stTarget = stTarget+ (delta*stepMult*mmRatio);
00209             }else if(edit==1){
00210                 z=z+delta;
00211                 if(z>1){z=0;}else if (z<0){z=1;}
00212                 stepMult= multMass[z]; 
00213             }else if(edit==2){
00214                 k=k+delta;
00215                 if(k>9){k=0;}else if (k<0){k=9;}
00216                 feedRate = feedMass[k];
00217                 stepPeriod=60000000/(feedRate*mmRatio);
00218             }
00219         }else{
00220             edit++;
00221             if(edit>2){edit=0;}
00222             wait_ms(100);
00223         }
00224         
00225         if(!pos1Button.read()){
00226             pos1=stPos;
00227             printTrPos();
00228             wait_ms(100);
00229         }
00230         
00231         if(!pos2Button.read()){
00232             stPos=0;
00233             stTarget = 0;
00234             printStPos();
00235             wait_ms(100);
00236         }
00237         
00238         if(!selectButton.read()){
00239             state++;
00240             if(state>1){state=0;}
00241             wait_ms(500);
00242         }
00243             
00244         wait_ms(encoderRATE);
00245         
00246     }
00247 }
00248 
00249 void feedToPos(){
00250     stepPeriod=60000000/(feedRate*mmRatio);
00251     stTarget = pos1;
00252     wait_ms(100);
00253 }
00254 void feedHandMode(){
00255     stepPeriod=stepRATE;
00256     wait_ms(100);
00257     }
00258 void feedToZero(){
00259     stepPeriod=60000000/(feedRate*mmRatio);
00260     stTarget = 0;
00261     wait_ms(100);
00262     }
00263 
00264 
00265 void stepper_thread() {
00266     while(1){
00267        if(stTarget>stPos){dirPin=1; dir=1;}else{dirPin=0;dir=-1;}
00268        
00269        if(stTarget!=stPos){
00270         stepPin=1;
00271         wait_us(2);
00272         stepPin=0;
00273         wait_us(stepPeriod);
00274         stPos=stPos+dir;
00275        } else{
00276            wait_ms(100);
00277            }
00278        
00279     }
00280 }
00281 
00282 
00283 int main() {
00284     pc.printf("startUp\r\n");  
00285     wheelPanButton.mode(PullUp);
00286     pos1Button.mode(PullUp);
00287     pos2Button.mode(PullUp);
00288     selectButton.mode(PullUp);
00289     switch1Button.mode(PullUp);
00290     switch2Button.mode(PullUp);
00291     switch3Button.mode(PullUp);
00292     pc.printf("ButtonIsPullUp\r\n");  
00293     
00294     switch1Button.fall(&feedToPos);
00295     switch2Button.fall(&feedHandMode);
00296     switch3Button.fall(&feedToZero);
00297     pc.printf("InteruptIsOn\r\n");  
00298     
00299     //wheelPanButton.set_samples(5);
00300     //wheelPanButton.set_debounce_us(5000);
00301     threadEnc.start(encoderPan_thread);
00302     pc.printf("encoderThreadIsRunning\r\n"); 
00303     wait_ms(500);
00304     threadStep.start(stepper_thread);
00305     pc.printf("stepperThreadIsRunning\r\n");  
00306     wait_ms(500);
00307 
00308     
00309     while(1){
00310 
00311         //ANGLE MODE    
00312         if ((state==stAngle)){
00313             //print screen
00314             if(state!=prevState){
00315                 printStAngle();
00316                 prevState=state;
00317             }
00318             if(prevAngle!=Angle){
00319                 printWheelPan();
00320                 prevAngle=Angle;
00321                 stTarget = Angle*stepMult;
00322                 }
00323                 
00324             if(prevWheelBut!=wheelBut){
00325                 //printWheelBut();
00326                 prevWheelBut=wheelBut;
00327                 }
00328             
00329 
00330         }
00331         //FEED MODE
00332         if ((state==stFeed)){
00333             //print screen
00334             if(state!=prevState){
00335                 printStFeed();
00336                 prevState=state;
00337             }
00338             //check encoder        
00339             if(stPrevPos!=stPos){
00340                 printStPos();
00341                 stPrevPos=stPos;
00342                 showStopPos =1;
00343             }else if(showStopPos){
00344                 printStPos();
00345                 showStopPos=0; 
00346             }else if(edit==1){
00347                 printMult();
00348             }else if(edit==2){
00349                 printFeed();
00350             }
00351             
00352 
00353             
00354                
00355             if(prevStepMult!=stepMult){
00356                 printMult();
00357                 prevStepMult=stepMult;
00358             }
00359             
00360             
00361         }
00362         //pitnTmpVar(stepPeriod);
00363         wait_ms(lcdRATE);
00364     }
00365 
00366 }
00367