// Ardunio Control of the S107G, Hak, and Kingco Helicopters // Derived from: kodek.pde - ver 1.0 - S107G IR packet transmitter // Modified by Andrew Barry, Dan Barry, Brandon Vasquez, Jacob Izraelevitz, Andy Pethan 2012-2020 // // Usage: attach IR LEDs to pin 13 and use the Serial Monitor to control the helicopter // --- Initial setup code (don't worry about this too much) --- // SELECT YOUR BRAND AND CHANNEL BY SETTING "channel" BELOW: #define S107_CHANNEL_A 0 // don't change these numbers #define S107_CHANNEL_B 1 #define HAK_CHANNEL_A 2 #define HAK_CHANNEL_B 3 #define HAK_CHANNEL_C 4 #define K_CHANNEL_A 5 #define K_CHANNEL_B 6 #define K_CHANNEL_C 7 byte channel = HAK_CHANNEL_A; // change this to brand and channel you have // For example: // byte channel = K_CHANNEL_B; // // --- end initial setup code --- /* * HoldCommand (for your reference) * Inputs: * yawIn: Turn left/right. * 0 = maximum left turn * 63 = no turn * 127 = maximum right turn * * pitchIn: pitch the helicopter to fly forwards and backwards * 0 = maxmimum backwards flight * 63 = hover * 127 = maximum forward flight * * throttleIn: speed of the rotors (go up and down or hover) * 0 = no throttle * 65 = approximate hover throttle * 127 = max throttle (will go up FAST!) * * holdTime: Time to hold this command for in milliseconds * 0 = do nothing * 500 = hold command for a half second * 1000 = hold for one second * * Outputs: * None. * * Example: * Hover for one half of a second * HoldCommand(63, 63, 77, 500) * 63; don't turn left or right * 63: don't move forward or backwards * 60: approximate hover throttle * 500: do this for 500ms (aka half a second) * **/ /* * This function is called when the button is pressed. */ void ButtonPressed() { // print to the serial port Serial.println(); Serial.println("You hit the button."); Serial.println(); // HoldCommand takes 4 numbers - yaw (0 to 127), pitch (0 to 127), throttle (0 to 127), time to hold (in milliseconds) // REMINDER: HoldCommand(yaw, pitch, throttle, time) // remove the comment slashes "//" below to make that code execute with the button! // HoldCommand(63, 63, 110, 500); // example take-off with lots of throttle! // HoldCommand(63, 63, 60, 2000); // hover for 2 seconds (2000ms) without tilting or turning // HoldCommand(63, 63, 40, 2000); // slow down for soft landing // modify these and add your own new lines of code to do cool things! // try flying in a square, flying to a destination, or a crazy pattern // consider also modifying the code to make it easier to update the throttle as your // helicopter battery drains -- try a variable and adding/subtracting from it } // // ----------------------------- // Implementation details below! // ----------------------------- // byte yawCmd, pitchCmd, throttleCmd; void serialEvent() // Called every time a command is recieved on the serial port { char cmd = Serial.read(); // Reads in a command from the serial port // print out what command was received Serial.println(); Serial.print("command received is "); Serial.println(cmd); switch (cmd) { case 'O': case 'o': case '0': Serial.print("Zeroing throttle. "); throttleCmd = 0; yawCmd = 63; pitchCmd = 63; break; case '5': // Attempt at hover throttle Serial.print("Got a throttle command."); throttleCmd = 60; // modify this slightly to make it easy to fly break; case '1': case '2': case '3': case '4': case '6': case '7': case '8': case '9': Serial.println("Got a throttle command."); throttleCmd = atoi(&cmd) * 14; // Single character, so we can go from 0 to 124 by inputting 0 to 9 in the serial monitor break; case 'A': case 'a': // Causes the helicopter to rotate counter-clockwise if (yawCmd - 5 >= 0) { yawCmd -= 5; } else { yawCmd = 0; } Serial.print("Yaw is "); Serial.println(yawCmd); break; case 'D': case 'd': // Causes the helicopter to rotate clockwise if (yawCmd + 5 <= 127) { yawCmd += 5; } else { yawCmd = 127; } Serial.print("Yaw is "); Serial.println(yawCmd); break; case 'W': case 'w': // Causes the helicopter to pitch forward if (pitchCmd + 5 <= 127) { pitchCmd += 5; } else { pitchCmd = 127; } Serial.print("Pitch is "); Serial.println(pitchCmd); break; case 'S': case 's': // Causes the helicopter to pitch backwards if (pitchCmd - 5 >= 0) { pitchCmd -= 5; } else { pitchCmd = 0; } Serial.print("Pitch is "); Serial.println(pitchCmd); break; case 'U': case 'u': // Causes the helicopter to inc in throttle if (throttleCmd + 2 <= 127) { throttleCmd += 2; } else { throttleCmd = 127; } break; case 'J': case 'j': // Causes the helicopter to dec in throttle if (throttleCmd - 6 >= 0) { throttleCmd -= 6; } else { throttleCmd = 0; } break; //case 'C': //case 'c': // Changes the channel A = 0, B = 1 // Serial.println("Changing channel"); // if (channel == 1) { // channel = 0; // } else { // channel = 1; // } // break; case 'R': case 'r': // Reset Serial.println("Resetting yaw and pitch"); yawCmd = 63; pitchCmd = 63; break; default: // Bad command Serial.println("Unknown command"); } Serial.print("Throttle is at "); Serial.println(throttleCmd); Serial.print("Trim is an extra +"); Serial.println(analogRead(A0)/60); } #define LED 13 // Pin connected to the infrared leds #define BUTTON_INPUT 8 #define TAKEOFF_THROTTLE 110 #define HOLDING_THROTTLE 58 #define DELAY_CONST 50 /* Setup runs once, when the Arduino starts */ void setup() { // Start the serial port communications Serial.begin(9600); // set the IR LED pin to be an output, take it to 0 volts pinMode(LED,OUTPUT); digitalWrite(LED,LOW); // configure the button input to be a pullup, // so we can connect the pin through the button // to ground pinMode(BUTTON_INPUT, INPUT_PULLUP); // initialize global command variables to be neutral. yawCmd = 63; // yaw: 0-127, 63 is no yaw pitchCmd = 63; // pitch: 0-127, 63 is no pitch throttleCmd = 0; // throttle: 0-127, 0 is no throttle // send the first serial communication Serial.println("throttle = 0, standing by for commands."); } /* * Function to send a packet to the helicopter * * Inputs: yaw, pitch, throttle, trim * * Yaw: 0-127, 63 = no yaw * Pitch: 0-127, 63 = no pitch * Throttle: 0-127, 0 = off * Trim: 0-127, 0 = no trim * * Outputs: * time it took to send the packet */ byte sendPacket_S107G(byte yaw, byte pitch, byte throttle) { pitch = 127-pitch; // reverse pitch so fowards is higher numbers yaw = 127-yaw; // reverse yaw to that left is lower and right is higher numbers static byte markL, countP, countR, one, zero, flag; static bool data; static const byte maskB[] = {1,2,4,8,16,32,64,128}; static byte dataP[4]; dataP[0] = yaw; dataP[1] = pitch; dataP[2] = throttle; //dataP[3] = trim; TODO remove markL = 77; countP = 4; countR = 8; one = 0; zero = 0; data = true; // flash the IR LED 77 times while(markL--) { digitalWrite(LED,LOW); delayMicroseconds(10); digitalWrite(LED,HIGH); delayMicroseconds(10); } // wait 2ms delayMicroseconds(1998); markL = 12; while(data) { // flash 12 times while(markL--) { digitalWrite(LED,LOW); delayMicroseconds(10); digitalWrite(LED,HIGH); delayMicroseconds(10); } markL = 12; flag = countR; if((dataP[4-countP] & maskB[--countR]) || (flag == 8 && countP == 2 && channel == S107_CHANNEL_B)) { one++; //Serial.print("1"); delayMicroseconds(688); } else { //Serial.print("0"); zero++; delayMicroseconds(288); } if(!countR) { countR = 8; countP--; //Serial.println("... next command"); } if(!countP) { data = false; } } while(markL--) { digitalWrite(LED,LOW); delayMicroseconds(10); digitalWrite(LED,HIGH); delayMicroseconds(10); } return((.1-.014296-one*.000688-zero*.000288)*1000); // in ms. } /* * Function that does the actual work of converting commands into * IR LED pulses and changes the pins in the appropriate manner. */ byte last_packet_num = 0; byte sendPacket_Hak(byte yaw, byte pitch, byte throttle) { int packetData[100]; int pulseNum; byte delayValue; int i; if (last_packet_num == 0) { delayValue = 94; last_packet_num ++; } else if (last_packet_num == 1) { delayValue = 132; last_packet_num ++; } else { delayValue = 170; last_packet_num = 0; } digitalWrite(LED, HIGH); // First is always 1 packetData[0] = 1; // throttle // 127 = zero throttle // 7 = full throttle // map from 0-127 to 7-127: int throttleOut = -120.0/127.0 * throttle + 127; packetData[1] = (throttleOut & 0b1000000) >> 6; packetData[2] = (throttleOut & 0b0100000) >> 5; packetData[3] = (throttleOut & 0b0010000) >> 4; packetData[4] = (throttleOut & 0b0001000) >> 3; packetData[5] = (throttleOut & 0b0000100) >> 2; packetData[6] = (throttleOut & 0b0000010) >> 1; packetData[7] = (throttleOut & 0b0000001); // yaw // map 0-127 to 17 - 0 int yawMagnitude = -abs(64-yaw) + 64; int yawOut = 15.0/63.0 * yawMagnitude; packetData[8] = (yawOut & 0b01000) >> 3; packetData[9] = (yawOut & 0b00100) >> 2; packetData[10] = (yawOut & 0b00010) >> 1; packetData[11] = (yawOut & 0b00001); // Direction bit packetData[16] = yaw < 63; // pitch int pitchMagnitude = -abs(64-pitch) + 64; int pitchOut = 15.0/63.0 * pitchMagnitude; packetData[12] = (pitchOut & 0b1000) >> 3; packetData[13] = (pitchOut & 0b0100) >> 2; packetData[14] = (pitchOut & 0b0010) >> 1; packetData[15] = (pitchOut & 0b0001); // Direction bit packetData[17] = pitch > 63; // yaw trim / yaw adjust (the little dial on the controller) // 6 bits packetData[18] = 0; packetData[19] = 1; packetData[20] = 0; packetData[21] = 1; packetData[22] = 1; packetData[23] = 1; if (channel == HAK_CHANNEL_A) { packetData[24] = 0; packetData[25] = 0; } else if (channel == HAK_CHANNEL_B) { packetData[24] = 1; packetData[25] = 0; } else { packetData[24] = 0; packetData[25] = 1; } // Compute checksum. int checksum = packetData[7] + 2*packetData[6] + 4*packetData[5] + 8*packetData[4] + 16*packetData[3] + 32*packetData[2] + 64*packetData[1] -68+ packetData[15] + 2*packetData[14] + 4*packetData[13] + 8*packetData[12] -15+ packetData[23] + 2*packetData[22] + 4*packetData[21] + 8*packetData[20] + 16*packetData[19] + 32*packetData[18] -10+ packetData[10] *32+ packetData[11]*16-48; packetData[26] = (checksum & 0b100000) >> 5; packetData[27] = (checksum & 0b010000) >> 4; packetData[28] = (checksum & 0b001000) >> 3; packetData[29] = (checksum & 0b000100) >> 2; packetData[30] = (checksum & 0b000010) >> 1; packetData[31] = (checksum & 0b000001); packetData[32] = 1; /* * Optional realtime display of packet data */ bool displayPacketData = false; if (displayPacketData) { for (i=0; i<33; i++) { Serial.print(packetData[i]); if ((i+1)%4 == 0) { Serial.print(" "); } } Serial.println(" "); } // Send the packet by flashing the LEDs. for (i=-1; i<33; i++) { if (i < 0) { pulseNum = 130; } else { // a "0" bit is 13 pulses and a "1" bit is 28 pulses if (packetData[i] == 1) { pulseNum = 25; } else { pulseNum = 12; } } // flash pulseNum times // a "0" bit is 16 pulses and a "1" bit is 32 pulses while(pulseNum--) { digitalWrite(LED,LOW); delayMicroseconds(9); digitalWrite(LED,HIGH); delayMicroseconds(9); } // Delay between bits depends on if it was a 1 or a zero. // Total time is 1ms. if (i < 0) { delayMicroseconds(1000); } else { if (packetData[i] == 1) { delayMicroseconds(320); } else { delayMicroseconds(680); } } } // The delay value between the end of this packet and the next one. return(delayValue); // in milliseconds (ms) } /* * Function that does the actual work of converting commands into * IR LED pulses and changes the pins in the appropriate manner. */ byte sendPacket_Kingco(byte yaw, byte pitch, byte throttle) { int packetData[100]; int pulseNum; byte channelDelayValue; int i; digitalWrite(LED, HIGH); // throttle // 0 = zero throttle // 63 = full throttle // map from 0-127 to 0-63: int throttleOut = 63.0/127.0 * throttle; packetData[0] = (throttleOut & 0b0100000) >> 5; packetData[1] = (throttleOut & 0b0010000) >> 4; packetData[2] = (throttleOut & 0b0001000) >> 3; packetData[3] = (throttleOut & 0b0000100) >> 2; packetData[4] = (throttleOut & 0b0000010) >> 1; packetData[5] = (throttleOut & 0b0000001); // yaw // map 0-127 to 0-15 // Max left is 7, minimum left is 0 // Max right is 15, minimum right is 9 // No yaw is 8 int yawOut; if (yaw > 55 && yaw < 71) { yawOut = 0; } else if (yaw > 63) { yawOut = 15.0/127.0 * yaw; } else { yawOut = -7.0/55.0 * yaw + 7; } packetData[6] = (yawOut & 0b01000) >> 3; packetData[7] = (yawOut & 0b00100) >> 2; packetData[8] = (yawOut & 0b00010) >> 1; packetData[9] = (yawOut & 0b00001); int yawChecksum = packetData[9] + packetData[8]*2 + packetData[7]*4 + packetData[6]*8; // Bits 10 and 11 are always zero packetData[10] = 0; packetData[11] = 0; // Trim, we'll use always zero packetData[12] = 0; packetData[13] = 0; packetData[14] = 0; packetData[15] = 0; packetData[16] = 0; packetData[17] = 0; // pitch // Map 0-127 to 0-15 // Values 1-7 are forward, 8-15 are reverse, 0 is no pitch int pitchOut; if (pitch < 71 && pitch > 55) { pitchOut = 0; } else if (pitch < 63) { // Backwards pitchOut = -7.0/55.0 * pitch + 15; } else { // Forwards pitchOut = 3.0/32.0 * pitch - 157.0/32.0; } packetData[18] = (pitchOut & 0b1000) >> 3; packetData[19] = (pitchOut & 0b0100) >> 2; packetData[20] = (pitchOut & 0b0010) >> 1; packetData[21] = (pitchOut & 0b0001); // Bits 22-27 are always 0 packetData[22] = 0; packetData[23] = 0; packetData[24] = 0; packetData[25] = 0; packetData[26] = 0; packetData[27] = 0; if (channel == K_CHANNEL_A) { packetData[28] = 0; packetData[29] = 1; channelDelayValue = 119; // milliseconds } else if (channel == K_CHANNEL_B) { packetData[28] = 1; packetData[29] = 1; channelDelayValue = 89; // milliseconds } else { packetData[28] = 1; packetData[29] = 0; channelDelayValue = 89; } // Bits 30-31 are always 1 packetData[30] = 1; packetData[31] = 1; //// TRIM IS ALWAYS 0 //trim = 0; //int checksum = (253-64*trim - 4*packetData[29] - 8*packetData[28] - 4*packetData[5]-8*packetData[4]-16*packetData[3]-32*packetData[2]-64*packetData[1]-127*packetData[0] - 64*yawChecksum-4*pitchOut); //if (checksum < 0) { //int checksum2 = abs(checksum) % 255; //checksum = 255-checksum2; //} int channelVal = 2*packetData[28] + packetData[29]; int forwardBackVal = 8*packetData[18] + 4*packetData[19] + 2*packetData[20] + packetData[21]; int throttleVal = packetData[5] + 2*packetData[4] + 4*packetData[3] + 8*packetData[2] + 16*packetData[1] + 32*packetData[0]; int checksum = 256-3*1-channelVal*4-4*forwardBackVal-64*packetData[17]-128*packetData[16]-packetData[15]-2*packetData[14]-4*packetData[13]-8*packetData[12]-64*packetData[9]-128*packetData[8]-packetData[7]-2*packetData[6]-4*throttleVal; if (checksum < 0) { int checksum2 = abs(checksum) % 256; checksum = 256-checksum2; } //int checksum = (253-64*trim - 4*packetData[29] - 8*packetData[28] - 4*packetData[5]-8*packetData[4]-16*packetData[3]-32*packetData[2]-64*packetData[1]-127*packetData[0]); packetData[32] = (checksum & 0b10000000) >> 7; packetData[33] = (checksum & 0b01000000) >> 6; packetData[34] = (checksum & 0b00100000) >> 5; packetData[35] = (checksum & 0b00010000) >> 4; packetData[36] = (checksum & 0b00001000) >> 3; packetData[37] = (checksum & 0b00000100) >> 2; packetData[38] = (checksum & 0b00000010) >> 1; packetData[39] = (checksum & 0b00000001); // Bits 40 and 41 are always zero packetData[40] = 0; packetData[41] = 0; /* * Optional realtime display of packet data */ bool displayPacketData = false; if (displayPacketData) { for (i=0; i<42; i++) { Serial.print(packetData[i]); if ((i+1)%4 == 0) { Serial.print(" "); } } Serial.println(" "); } int shortDelay = 400; // microseconds int longDelay = 820; int smallNumPeaks = 15; int largeNumPeaks = 31; int delayAmount; int this_pair; // Send the packet by flashing the LEDs. for (i=-2; i<42; i+=2) { if (i < 0) { pulseNum = 60; delayAmount = shortDelay; } else { // Bits are sent in groups of two. this_pair = packetData[i]*2 + packetData[i+1]; if (this_pair == 0) { pulseNum = smallNumPeaks; delayAmount = shortDelay; // microseconds } else if (this_pair == 1) { pulseNum = smallNumPeaks; delayAmount = longDelay; } else if (this_pair == 2) { pulseNum = largeNumPeaks; delayAmount = shortDelay; } else { pulseNum = largeNumPeaks; delayAmount = longDelay; } } // flash pulseNum times // a "0" bit is 16 pulses and a "1" bit is 32 pulses while(pulseNum--) { digitalWrite(LED,LOW); delayMicroseconds(9); digitalWrite(LED,HIGH); delayMicroseconds(8); } // Delay between bits depends on if it was a 1 or a zero. // Total time is 1ms. delayMicroseconds(delayAmount); } // The delay value between the end of this packet and the next one. return(channelDelayValue); // in milliseconds (ms) } byte sendPacket(byte yaw, byte pitch, byte throttle) { int throt_int = throttle + (analogRead(A0) / 60); // includes trim addition if (throttle != 0) { // kill command should actually kill it if (throt_int > 127) throttle = 127; // throttle is a byte, so use the int to avoid overflow else if (throt_int < 0) throttle = 0; else throttle = throt_int; } if (channel == S107_CHANNEL_A || channel == S107_CHANNEL_B) { return sendPacket_S107G(yaw, pitch, throttle); } else if (channel == HAK_CHANNEL_A || channel == HAK_CHANNEL_B || channel == HAK_CHANNEL_C) { return sendPacket_Hak(yaw, pitch, throttle); } else if (channel == K_CHANNEL_A || channel == K_CHANNEL_B || channel == K_CHANNEL_C) { return sendPacket_Kingco(yaw, pitch, throttle); } else { Serial.print("\nUnknown channel value: "); Serial.print(channel); Serial.print(". You need to set the \"channel\" variable."); } } void TestCopter() // Small function that tests the helicopters yaw, pitch and throttle at once { yawCmd = 15; pitchCmd = 15; throttleCmd = TAKEOFF_THROTTLE; HoldCommand(yawCmd, pitchCmd, throttleCmd, 500); throttleCmd = 0; yawCmd = 63; pitchCmd = 63; } /* * HoldCommand * Inputs: * yawIn: Turn left/right. * 0 = maximum right turn * 63 = no turn * 127 = maximum left turn * * pitchIn: pitch the helicopter to fly forwards and backwards * 0 = maxmimum backwards flight * 63 = hover * 127 = maximum forwards flight * * throttleIn: speed of the rotors (go up and down or hover) * 0 = no throttle * 77 = apporximate hover throttle * 127 = max throttle (will go up FAST!) * * holdTime: Time to hold this command for in milliseconds * 0 = do nothing * 500 = hold command for a half second * 1000 = hold for one second * * Outputs: * None. * * * */ void HoldCommand(int yawIn, int pitchIn, int throttleIn, int holdTime) { // print out status messages to the serial port Serial.print("HoldingCommand running:\n\t---------------------------------------------------\n"); Serial.print("\tYaw\tPitch\tThrottle\tTrim\tTime (ms)\n\t---------------------------------------------------\n\t"); Serial.print(yawIn); Serial.print("\t"); Serial.print(pitchIn); Serial.print("\t"); Serial.print(throttleIn); Serial.print("\t\t"); Serial.print((analogRead(A0) / 60)); // trim calculation Serial.print("\t"); Serial.println(holdTime); // initialize variables int delayAmount = holdTime/DELAY_CONST; int packetDelay; // loop until we're done holding the command while (holdTime > 0) { // check to see if we should abort because of a message // on the serial port if (Serial.available() == true) { // abort Serial.println("HOLD COMMAND ABORTED"); break; } // send a packet to the heliocopter containing the command // we're holding packetDelay = sendPacket(yawIn, pitchIn, throttleIn); // remember how long we've held this for and subtract // from our total time holdTime = holdTime - packetDelay; // wait until the next packet delay(packetDelay); // delay a bit more delay(delayAmount); holdTime = holdTime - delayAmount; } // print out that we're done Serial.println("done."); Serial.println(); } void loop() { // Note that serialEvent() gets called on each path of the loop // and runs if there is data at the serial port delay(sendPacket(yawCmd,pitchCmd,throttleCmd)); // check the button (we check for LOW since we've configured it to be pulled high) if (digitalRead(BUTTON_INPUT) == LOW) { ButtonPressed(); } }