Пример #1
0
//-------------------------------------------------------------
void ofApp::handleMessage(ofxOscMessage m) {
    string send_string;
    string address_string;
    string ip_string;
    
    ip_string = "/" + ofSplitString( ofToString(m.getRemoteIp()), ".")[3];
    
    // check for battery message
    if(m.getAddress() == "/battery"){
        if(m.getArgType(0) == OFXOSC_TYPE_FLOAT){
            // construct OSC message to send
            sendBattery( ip_string, m.getArgAsFloat(0));
        }
    }
    
    // check for blob message
    else if(m.getAddress() == "/inputs/serial/1") {
        for(int i = 0; i < m.getNumArgs(); i++){
            // get the argument type
            if(m.getArgType(i) == OFXOSC_TYPE_BLOB){
                string blob = m.getArgAsBlob(0);
                vector<string> parameters = ofSplitString(blob, ",");
                if( parameters.size() >= 5 ) {
                    int distance = ofToInt(parameters[0]);
                    int velocity = ofToInt(parameters[1]);
                    int azimuth = ofToInt(parameters[2]);
                    int elevation = ofToInt(parameters[3]);
                    float angularRate = ofToFloat(parameters[4]);
                    
                    // send osc messages
                    sendDistance(ip_string, distance);
                    sendVelocity(ip_string, velocity);
                    sendAzimuth(ip_string, azimuth);
                    sendElevation(ip_string, elevation);
                    sendAngularRate(ip_string, angularRate);
                }
            }
        }
    }
}
//#pragma code highPriorityInterruptAddress=0x0008
//void high_interrupt(void)
//{
//    _asm GOTO highPriorityIsr _endasm
//}
//
//#pragma code
void main(void){
  


  //Default
  TRISD = 0x00;         //set port D signals to output
  menu_ref_1=1;
  menu_ref_2=0;
  
  
  /*zone of disgust*/
  
  values[MAXSPEED]=100;        //Max speed
  values[PIDGAINS]=100;        //PID Gains
  values[MAXYAW]=100;        //Max Yaw
  values[IRSAMPE]=10;         //ir_samp_e;
  values[IRSAMPR]=20;         //ir_samp_r;
  values[IRRAW]=60;         //ir_raw
  values[IRAVG]=10;         //ir_avg;
  
  /*****************/
  
  setupSerial();
  Lcd_Init();
  ADC_setup();
  Button_Setup();
  welcome();
  LCD_disp(menu_ref_1, menu_ref_2);

  RUN=0;
  while(1){

  
//NORMAL OPERATION  
    while(RUN==0){
        
        
        if(PORTDbits.RD0 == 0)
        {
            mode_button = 1;
        }
        
        if(PORTDbits.RD1 == 0)
        {
            motor_button = 1;
        }
        
        


        if (mode_button==1){                  //Mode change sequence
                checkMode(menu);
                menu_ref_2=0;         //Default menu ref, should only be SPEED
                delayms(200);
                mode_button=0;
                LCD_disp(menu_ref_1, menu_ref_2);
        }

        delayms(200);       //Used to prevent double tapping
        //possibly new function
        switchChannels(0);
        joy_x = doADC();
        switchChannels(1);
        joy_y = doADC();   
                         
      
        if(menu_ref_1==MANUAL)   //IF IN MANUAL
        {
            
           
            if(joy_x<=LEFT){      //User pushes right joystick left
                if(values[menu_ref_2]>0){
                    values[menu_ref_2]=values[menu_ref_2]-5;          //decrement value by 5%
                    GLOBAL_MAX_SPEED = values[menu_ref_2];
                    sendMaxSpeed();
                }
                
                LCD_disp(menu_ref_1, menu_ref_2);
                delayms(200);     //Delay to prevent flickering, and also to prevent 100-0 increments

            }
            else if (joy_x>=RIGHT){     //User pushes right joystick right
                if(values[menu_ref_2]<100){
                
                    values[menu_ref_2]=values[menu_ref_2]+5;          //increment value by 5%
                    GLOBAL_MAX_SPEED = values[menu_ref_2];
                    sendMaxSpeed();
                }
                
                LCD_disp(menu_ref_1, menu_ref_2);
                delayms(200);
            }   
        }  

        else if(menu_ref_1==FACTORY){            //IF IN FACTORY MODE
            if (joy_y>=UP){            //User pushes left joystick UP  
                //Circular selection
                if(menu_ref_2==0){
                  menu_ref_2=4;
                }
                else{
                  menu_ref_2--;             //switched the order of this to prevent overflow
                }
                LCD_disp(menu_ref_1, menu_ref_2);
                delayms(250);               //Arbitrary delay of 250 milliseconds
            }
            else if (joy_y<=DOWN){      //User pushes left joystick DOWN
                menu_ref_2++;
                //Circular selection
                if(menu_ref_2>=7){
                  menu_ref_2=1;
                }
                LCD_disp(menu_ref_1, menu_ref_2);
                delayms(250);
                

            }
            else if (joy_x<=LEFT){      //User pushes right joystick left
                
                if(values[menu_ref_2]>0){
                    values[menu_ref_2]=values[menu_ref_2]-5;          //decrement value by 5%
                }
                
                LCD_disp(menu_ref_1, menu_ref_2);
                delayms(255);
                delayms(200);

            }
            else if (joy_x>=RIGHT){     //User pushes right joystick right
                
                if(values[menu_ref_2]<100){
                    values[menu_ref_2]=values[menu_ref_2]+5;          //decrement value by 5%
                }
                
                LCD_disp(menu_ref_1, menu_ref_2);
                delayms(255);
                delayms(200);

            }            
        }  
        
        //All other menu_ref_1's do not have values displayed       
        
        if (motor_button==1){
            motor_button=0;
            RUN=1;
            
        }
        //send here
    }
    
    /*Menu on setup*/
    if (RUN==1){
        on_setup();
        GLOBAL_RUN = RUN;
        sendRun();
        //MOTOR ON GOES AFTER
        motor_button=0;
    }
      
      /*MOTOR ON BEHAVIOUR*/
    while(RUN==1){
        if(PORTDbits.RD1 == 0)
        {
            motor_button = 1;
        }
        if (motor_button==1){      //At interrupt, stop motor and return to menu
            motor_button=0;
            RUN=0;
            GLOBAL_RUN = RUN;
            sendRun();
            
        }
        switchChannels(0);
        //GLOBAL_VELOCITY = doADC();    GLOBAL_VELOCITY = doADC() //Get joystick values
        joy_y = doADC();
        GLOBAL_VELOCITY = joy_y;
        
        switchChannels(1);
        joy_x = doADC();
        GLOBAL_OMEGA = joy_x;
        
        sendVelocity();
        sendOmega();
        
        //send command here
    }
      
      /*MOTOR OFF MESSAGE*/ 
    RUN=0;
      //MOTOR OFF FUNCTION GOES BEFORE MESSAGE
    LCD_title(t5);         //"Whoa!"
      
    delays(2);    //safety delay    

    LCD_disp(menu_ref_1, menu_ref_2); //Return to normal display
      
      
  }
  
  
  
}