Robotics Institute 

University of Michigan, Ann Arbor

© 2017 by Bruce Personal Website. 

Updated on 8/21/2017

Arduino Code

November 12, 2015

/*homing()
 * Will connect an EE-SX671 use to set deg 0
 */    
 void homing()
{
  int trig = 0;
  for(;;)
    {
        digitalWrite(stepps, HIGH);   
        delayMicroseconds(100);            
        digitalWrite(stepps, LOW);
        delayMicroseconds(100);      //sending a "Step" plus
        trig = digitalRead(homingP); //EE-SX671 
        if(trig != 0)
            {
                break;
            }
    }
}

/*heartbeat()
 * Mavlink need a specific pack stream to let the system know that the link was still up. 
 * It need two heartbeat pack in two second in order to stay "alive". ;)
 */
 void heartbeat()
{
    mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    Serial.write(buf, len);
}

/*RC()
 * RC override is the way we control the aircraft.
 * 1000 = 0%
 * 2000 = 100%
 */
void RC(uint16_t ch1)
{
    mavlink_msg_rc_channels_override_pack(sysid,compid, &msg, target_sys,target_com,ch1,0,0,0,0,0,0,0);
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
    Serial.write(buf, len);
}

/*getglobalDeg()
 * not in use for now. 
 */
/*float getglobalDeg(float nowdir)
{   
    float heading=0;
    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
    if(msg.msgid==MAVLINK_MSG_ID_VFR_HUD)
    {
    heading=mavlink_msg_vfr_hud_get_heading(&msg);
    }
    else{}
    }
    else{}
    if(nowdir>=180)
    {
        heading= -1*((nowdir+heading)%360);
    }
    if(nowdir<180)
    {
        heading= ((nowdir+heading)%360);
    }
    return heading;
}
*/

/*TURN()
 * Useing mav_cmd_condition_yaw to let aircraft turn relatively 
 */
void TURN(float deg)

{
mavlink_msg_command_long_pack(sysid,compid, &msg, target_sys,target_com,115,0,deg,0,1,1,0,0,0);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
Serial.write(buf, len);
}
void setup() {
  Serial.begin(57600); // Start serial communications
  pinMode(stepps, OUTPUT); // Set Step pin as OUTPUT
  pinMode(dir, OUTPUT); // 
  pinMode(en, OUTPUT); // 
  pinMode(safety, INPUT);
  pinMode(homingP, INPUT); // 
  digitalWrite(dir, HIGH); // set motor ditrction to CW 
  //!!!!!!!!!!!!!!!!!!!!!!!!
  //!!!!!!!!!!!!!!!!!!!!!!!!
  //!!!!!!!!!!!!!!!!!!!!!!!!
  //!!!!!!!!!!!!!!!!!!!!!!!!
  //!!!!!!!!!!!!!!!!!!!!!!!!
  //!!!!!!!!!!!!!!!!!!!!!!!!
  //IF the direction is changed, we must change the direction of mav_cmd_condition_yaw as well, or the result will be wrong.


  digitalWrite(en, LOW); // enableing the motor
  delay(100);
}

//As the main loop in C.
void loop() 
{
  
  homing(); //well...HOMING~!!
  delay(2000);
  for(;;)
    {
        if(pulseIn(safety, HIGH)>1500) //for safety
        {
        heartbeat(); //if safety swith is of
        }
    
        int volmax = 0;
        int vnow =0;
        float maxdir = 0;
        for (int i;i<6400;i++)//1 rotation is 32*200=6400steps
            {
                //c = Serial.read(); //not in use for now
                vnow =analogRead(A0); 
                if(volmax<vnow)
                    {
                        volmax=vnow;
                        maxdir = (i*360/6400); ///////trun the Steps back to deg
                    }
                digitalWrite(stepps, HIGH);   // turn the LED on (HIGH is the voltage level)
                delayMicroseconds(100);              // wait for a second
                digitalWrite(stepps, LOW);    // turn the LED off by making the voltage LOW
                delayMicroseconds(100);
            }
        if (maxdir<=5||maxdir>=355) // set 355~5 deg as dead zone
            {
                RC(1750); //if the aircraft is heading the place where we want. It will fly forward.
            }
        else
            {
                RC(1500); //if the aircraft isn't heading the place where we want. It will stop flying forward and start to turn.
                TURN(maxdir); //turn~
            }
    }
}
 

Please reload