diff --git a/3dMouseTestSerialExtendedGaits.py b/3dMouseTestSerialExtendedGaits.py new file mode 100644 index 0000000..b1ada56 --- /dev/null +++ b/3dMouseTestSerialExtendedGaits.py @@ -0,0 +1,207 @@ +##0 - Right/Left +##1 - Back/Forward +##2 - Down/Up +##3 - Rotation around z +##4 - Rotation around x +##5 - Rotation around y + + +import pygame +import serial +import time +import sys +import math + +ser = serial.Serial( + port='COM12', + baudrate=38400, + stopbits=serial.STOPBITS_ONE, + parity=serial.PARITY_NONE, + bytesize=serial.EIGHTBITS + + #write_timeout=2 +) + + +def text_render(screen,text,pos,color = (0,0,0),size=50): + try: + text = str(text) + font = pygame.font.SysFont('Comic Sans MS',size) + text = font.render(text, True, color) + screen.blit(text,pos) + except Exception, e: + print 'Font Error' + raise e + + +def translate(value, leftMin, leftMax, rightMin, rightMax): + # Figure out how 'wide' each range is + leftSpan = leftMax - leftMin + rightSpan = rightMax - rightMin + + # Convert the left range into a 0-1 range (float) + valueScaled = float(value - leftMin) / float(leftSpan) + + # Convert the 0-1 range into a value in the right range. + return rightMin + (valueScaled * rightSpan) + + + + +print "Pygame Loaded" +from pygame.locals import * +ser.flushOutput() + + +pygame.init() +DisplayWidth = 800 +DisplayHeight = 400 +DisplaySurface = pygame.display.set_mode((DisplayWidth,DisplayHeight)) + +joystick_count = pygame.joystick.get_count() +print ("There is ", joystick_count, "joystick/s") +if joystick_count == 0: + print ("Error, I did not find any joysticks") +else: + SpaceMouse = pygame.joystick.Joystick(1) + SpaceMouse.init() + print (SpaceMouse.get_name()) + print (SpaceMouse.get_numaxes()) + print (SpaceMouse.get_numbuttons()) + + NumAxes = SpaceMouse.get_numaxes() + NumButtons = SpaceMouse.get_numbuttons() +print("Started") + + + + +time.sleep(.25) +print("LoopStart"); +while True: + ser.flushInput #discard anything in the serial buffer + #while (ser.inWaiting > 0): + # print ser.read() + + for event in pygame.event.get(): + if event.type == QUIT: + print("quit") + ser.close() + pygame.quit() + sys.exit() + #get inputs + AxisX = SpaceMouse.get_axis(0) + AxisY = -SpaceMouse.get_axis(1) + AxisZ = -SpaceMouse.get_axis(2) + AxisRx = -SpaceMouse.get_axis(5) + AxisRy = SpaceMouse.get_axis(4) + AxisRz = -SpaceMouse.get_axis(3) + + AxisList = [AxisX, AxisY, AxisZ, AxisRx, AxisRy, AxisRz] + + #get keyboard keys + keydown=pygame.key.get_pressed() + Button1 = keydown[pygame.K_1] + Button2 = keydown[pygame.K_2] + Button3 = keydown[pygame.K_3] + Button4 = keydown[pygame.K_4] + Button5 = keydown[pygame.K_5] + Button6 = keydown[pygame.K_6] + Button7 = keydown[pygame.K_7]#SpaceMouse.get_button(1) + Button8 = keydown[pygame.K_8] + + #scale inputs + AxisX = int(translate(AxisX, -1, 1, 3, 253)+.5) + AxisY = int(translate(AxisY, -1, 1, 3, 253)+.5) + AxisZ = int(translate(AxisZ, -1, 1, 3, 253)+.5) + AxisRx = int(translate(AxisRx, -1, 1, 3, 253)+.5) + AxisRy = int(translate(AxisRy, -1, 1, 3, 253)+.5) + AxisRz = int(translate(AxisRz, -1, 1, 3, 253)+.5) + + OutByte1 = int(255) #255 + OutByte2 = int(170) #255 + OutByte3 = AxisX #X axis + OutByte4 = AxisY #Y axis + OutByte5 = AxisZ #Z axis + OutByte6 = AxisRx #Rx + OutByte7 = AxisRy #Ry + OutByte8 = AxisRz #Rz + OutByte9 = ((Button1*1)+(Button2*2)+(Button3*4)+(Button4*8)+(Button5*16)+(Button6*32)+(Button7*64)+(Button8*128)) #Buttons + OutByte10 = (1)*16+(0) #UserCommand(high nibble)+ UserData1 (low nibble) + OutByte11 = 1 #UserData2 + OutByte12 = int(255-(int((OutByte3+OutByte4+OutByte5+OutByte6+OutByte7+OutByte8+OutByte9+OutByte10+OutByte11)%256))) #checksum + + + #set extended byte commands + #reset + if keydown[pygame.K_r]: + OutByte10 = (0)*16+(0) + #gaits + if keydown[pygame.K_9]: + OutByte10 = (5)*16+(0) #GaitSelect/2x2 + + if keydown[pygame.K_0]: + OutByte10 = (5)*16+(1) #GaitSelect/crawl + + + ser.write(chr(OutByte1)) + #print"1Send " + #print(OutByte1) + ser.write(chr(OutByte2)) + #print"2Send" + #print(OutByte2) + ser.write(chr(OutByte3)) + #print"3Send" + #print(OutByte3) + ser.write(chr(OutByte4)) + #print"4Send" + #print(OutByte4) + ser.write(chr(OutByte5)) + #print"5Send" + #print(OutByte5) + ser.write(chr(OutByte6)) + #print"6Send" + #print(OutByte6) + ser.write(chr(OutByte7)) + #print"7Send" + #print(OutByte7) + ser.write(chr(OutByte8)) + #print"8Send" + #print(OutByte8) + ser.write(chr(OutByte9)) + #print"9Send" + #print(OutByte9) + ser.write(chr(OutByte10)) + #print"10Send" + #print(OutByte10) + ser.write(chr(OutByte11)) + #print"11Send" + #print(OutByte11) + ser.write(chr(OutByte12)) + #print"12Send" + #print(OutByte12) + ser.flush() + time.sleep(.1) + + + DisplaySurface.fill((0,0,0)) + pygame.draw.circle(DisplaySurface,(255,255,255),(DisplayWidth-15,15),10,0) + + for Axis in range(0,NumAxes): + print Axis,", ", SpaceMouse.get_axis(Axis) + AxisDistance = (DisplayWidth)/NumAxes + pygame.draw.circle(DisplaySurface,(255,255,255),(int(25+((Axis+.5)*AxisDistance)),DisplayHeight/4 - int(AxisList[Axis]*(((DisplayHeight/2)-50)/2))),5,0) + print"" + + #add text for Commander Packets + CommandPackets = str(str(OutByte1).zfill(3)+" "+str(OutByte2).zfill(3)+" "+str(OutByte3).zfill(3)+" "+str(OutByte4).zfill(3)+" "+str(OutByte5).zfill(3)+" "+str(OutByte6).zfill(3)+" "+str(OutByte7).zfill(3)+" "+str(OutByte8).zfill(3)+" "+str(OutByte9).zfill(3)+" "+str(OutByte10).zfill(3)+" "+str(OutByte11).zfill(3)+" "+str(OutByte12).zfill(3)) + text_render(DisplaySurface,CommandPackets,(15,250),(0,255,0),25) + HexPackets = str("0x%x" % OutByte1 +" "+ "0x%x" % OutByte2+" "+ "0x%x" % OutByte3+" "+ "0x%x" % OutByte4+" "+ "0x%x" % OutByte5+" "+ "0x%x" % OutByte6+" "+ "0x%x" % OutByte7+" "+ "0x%x" % OutByte8+" "+ "0x%x" % OutByte9+" "+ "0x%x" % OutByte10+" "+ "0x%x" % OutByte11+" "+ "0x%x" % OutByte12) + text_render(DisplaySurface,HexPackets,(15,275),(0,255,0),15) + + pygame.draw.circle(DisplaySurface,(0,255,255*Button1),(300,600),5,0) + + pygame.display.update() + time.sleep(.1) + + diff --git a/HexapodCodeV3-181208a.zip b/HexapodCodeV3-181208a.zip new file mode 100644 index 0000000..27303c8 Binary files /dev/null and b/HexapodCodeV3-181208a.zip differ diff --git a/HexapodCodeV3/a_presetup.ino b/HexapodCodeV3/a_presetup.ino index a1d76ac..45cd277 100644 --- a/HexapodCodeV3/a_presetup.ino +++ b/HexapodCodeV3/a_presetup.ino @@ -1,3 +1,4 @@ +//presetup //Enable return data for userSerial #define UserSerialTransmit @@ -139,54 +140,54 @@ BioloidControllerEx bioloid = BioloidControllerEx(); const int DXLServoLimits[NUM_LEGS][NUM_SERVOS_PER_LEG][3] = { { { - 538,759,2 } + 538,759,2 } ,{ - 183,813,3 } + 183,813,3 } ,{ - 39,941,4 } + 39,941,4 } ,{ - 205,820,5 } + 205,820,5 } ,{ - 205,820,6 } + 205,820,6 } } , { { - 280,469,7 } + 280,469,7 } ,{ - 193,809,8 } + 193,809,8 } ,{ - 56,930,9 } + 56,930,9 } ,{ - 211,814,10 } + 211,814,10 } ,{ - 211,814,11 } + 211,814,11 } } , { { - 543,756,12 } + 543,756,12 } ,{ - 186,814,13 } + 186,814,13 } ,{ - 44,926,14 } + 44,926,14 } ,{ - 188,817,15 } + 188,817,15 } ,{ - 188,817,16 } + 188,817,16 } } , { { - 262,468,17 } + 262,468,17 } ,{ - 182,824,18 } + 182,824,18 } ,{ - 69,941,19 } + 69,941,19 } ,{ - 208,812,20 } + 208,812,20 } ,{ - 211,814,21 } + 211,814,21 } } }; @@ -196,22 +197,22 @@ const int DXLServoLimits[NUM_LEGS][NUM_SERVOS_PER_LEG][3] = { #define NUM_PWM_SERVOS 6 const int PWMServoLimits[NUM_PWM_SERVOS][3] = {//order min,max,id { - 0,180,0 } + 0,180,0 } , { - 0,180,1 } + 0,180,1 } , { - 0,180,2 } + 0,180,2 } , { - 0,180,3 } + 0,180,3 } , { - 0,180,4 } + 0,180,4 } , { - 0,180,5 } + 0,180,5 } }; float LegDynamixels[NUM_LEGS][NUM_SERVOS_PER_LEG]{ @@ -238,11 +239,26 @@ float PWMServoPos[NUM_PWM_SERVOS]={ #define Leg3InitY -120 #define Leg3InitZ -100 +const int InitialPositions [NUM_LEGS] [3] = {//x,y,z + { + -120, 120, -100 } + ,//Leg0 + { + 120, 120, -100 } + ,//Leg1 + { + 120, -120, -100 } + ,//Leg2 + { + -120, -120, -100 } +};//Leg3 + + //Dimensions used in leg calculations -#define CoxaLength 50 -#define FemurLength 93 -#define TibiaLength 97 -#define TharsusLength 25 +#define COXA_LENGTH 50 +#define FEMUR_LENGTH 93 +#define TIBIA_LENGTH 97 +#define TARSUS_LENGTH 25 //Dimensions used to convert to local leg coords #define CogX 52 // distance that the legs are in left and right of the COG @@ -250,9 +266,13 @@ float PWMServoPos[NUM_PWM_SERVOS]={ //Variable used to output from functions unsigned long Tim1; -int GaitGenOut[NUM_LEGS][3]; // For GaitGen -int BodyModOut[NUM_LEGS][3]; // for BodyMod -int LegCoordsOut[NUM_LEGS][3];// For LegCoordsOut -float LegCalculateOut[NUM_LEGS][NUM_SERVOS_PER_LEG];// For LegCalculateOut +int GaitGenOut[NUM_LEGS][3]; // For GaitGen [leg] [x,y,z] +int BodyModOut[NUM_LEGS][3]; // for BodyMod [leg] [x,y,z] +int LegCoordsOut[NUM_LEGS][3];// For LegCoordsOut, [leg] [x,y,z] +//float LegCalculateOut[NUM_LEGS][NUM_SERVOS_PER_LEG];// For LegCalculateOut, [leg] [servo] //not used +float LegGlobalSpherical[NUM_LEGS][3]; //For GaitGen/SwerveSteer, [leg] [Azimuth, Elevation, Bank] +float LegLocalSpherical[NUM_LEGS][3]; //For LocalSpherical, [leg] [Azimuth, Elevation, Bank] + + diff --git a/HexapodCodeV3/b_setup.ino b/HexapodCodeV3/b_setup.ino index bcc0bde..3851b31 100644 --- a/HexapodCodeV3/b_setup.ino +++ b/HexapodCodeV3/b_setup.ino @@ -1,3 +1,4 @@ +//setup void setup(){ diff --git a/HexapodCodeV3/c_loop.ino b/HexapodCodeV3/c_loop.ino index 138dec0..7a2e520 100644 --- a/HexapodCodeV3/c_loop.ino +++ b/HexapodCodeV3/c_loop.ino @@ -1,3 +1,4 @@ +//loop void loop() { Tim1 = millis(); @@ -13,25 +14,25 @@ void loop() { switch (MoveMode){ case MOVE_MODE_WALK_PERIODIC: GaitGen1(GaitPeriod,Tim1,GaitMoveX,GaitMoveY,GaitStepHeight,GaitMoveZrot); + BodyMod(GaitBodyX,GaitBodyY,GaitBodyZ,GaitBodyPitch,GaitBodyRoll,GaitBodyYaw); + LegCoords(); + LegCalculate(); break; case MOVE_MODE_CRAWL_PERIODIC: GaitGen2(GaitPeriod,Tim1,GaitMoveX,GaitMoveY,GaitStepHeight,GaitMoveZrot); + BodyMod(GaitBodyX,GaitBodyY,GaitBodyZ,GaitBodyPitch,GaitBodyRoll,GaitBodyYaw); + LegCoords(); + LegCalculate(); break; - /* - case MOVE_MODE_SWERVE - SwerveSteer(GaitMoveX, GaitMoveY, GaitMoveZrot, GaitStepHeight, GaitStepHeight) + case MOVE_MODE_SWERVE: + SwerveSteer(GaitMoveX, GaitMoveY, GaitMoveZrot, GaitStepHeight, GaitStepHeight); + BodyMod(GaitBodyX,GaitBodyY,GaitBodyZ,GaitBodyPitch,GaitBodyRoll,GaitBodyYaw); + LegCoords(); + LocalSpherical(); + LegPlaceSixCalculate(); break; - */ - } - BodyMod(GaitBodyX,GaitBodyY,GaitBodyZ,GaitBodyPitch,GaitBodyRoll,GaitBodyYaw); - LegCoords(); - LegCalculate(); - - for(int i = 0; i < NUM_LEGS; i++){//Update LegDynamixels - for(int j = 0; j < NUM_SERVOS_PER_LEG; j++){ - LegDynamixels[i][j] = LegCalculateOut[i][j]; - } } + //sphericalCoords goes here diff --git a/HexapodCodeV3/d0_gait1.ino b/HexapodCodeV3/d0_gait1.ino index 19b8ec9..bab4a6a 100644 --- a/HexapodCodeV3/d0_gait1.ino +++ b/HexapodCodeV3/d0_gait1.ino @@ -1,3 +1,4 @@ +//gait1 //This code covers the basic Gait Generator void GaitGen1(int Period, int Cycle, int Xtrans, int Ytrans, int Ztrans, float ZTurn){ //Sale the inputs @@ -9,7 +10,7 @@ void GaitGen1(int Period, int Cycle, int Xtrans, int Ytrans, int Ztrans, float Z float Generator1 = ((4.0 *(abs((Cycle%Period)-(Period/2))-(Period/4)))/Period); float Generator2 = Generator1; int GeneratorZ = (-1*((abs((Cycle%Period)-(Period/2)))/((Cycle%Period)-(Period/2)))); - float Theta1 = (ZTurn * 3.14 / 4 *Generator1); + float Theta1 = (ZTurn * PI / 4 *Generator1); //Type1 and Type two are added to legs to produce movement float Type1[3] = { @@ -21,28 +22,28 @@ void GaitGen1(int Period, int Cycle, int Xtrans, int Ytrans, int Ztrans, float Z //First pass does gait generation for translation int Output1[NUM_LEGS][3] = { { //Leg 0 - (Leg0InitX + Type1[0]),//X (Output[0][0]) - (Leg0InitY + Type1[1]),//Y (Output[0][1]) - (Leg0InitZ + Type1[2]) //Z (Output[0][2]) + (InitialPositions[0][0] + Type1[0]),//X (Output[0][0]) + (InitialPositions[0][1] + Type1[1]),//Y (Output[0][1]) + (InitialPositions[0][2] + Type1[2]) //Z (Output[0][2]) } , { //Leg 1 - (Leg1InitX + Type2[0]),//X - (Leg1InitY + Type2[1]),//Y - (Leg1InitZ + Type2[2]) //Z + (InitialPositions[1][0] + Type2[0]),//X + (InitialPositions[1][1] + Type2[1]),//Y + (InitialPositions[1][2] + Type2[2]) //Z } , { //Leg 2 - (Leg2InitX + Type1[0]),//X - (Leg2InitY + Type1[1]),//Y - (Leg2InitZ + Type1[2]) //Z + (InitialPositions[2][0] + Type1[0]),//X + (InitialPositions[2][1] + Type1[1]),//Y + (InitialPositions[2][2] + Type1[2]) //Z } , { //Leg 3 - (Leg3InitX + Type2[0]),//X - (Leg3InitY + Type2[1]),//Y - (Leg3InitZ + Type2[2]) //Z + (InitialPositions[3][0] + Type2[0]),//X + (InitialPositions[3][1] + Type2[1]),//Y + (InitialPositions[3][2] + Type2[2]) //Z } , }; diff --git a/HexapodCodeV3/d1_gait2.ino b/HexapodCodeV3/d1_gait2.ino index 3f600a1..1086384 100644 --- a/HexapodCodeV3/d1_gait2.ino +++ b/HexapodCodeV3/d1_gait2.ino @@ -1,19 +1,22 @@ +//gait2 //This code covers the crawl Gait Generator //WIP -const uint8_t GaitLegOffset[NUM_LEGS] = {0,2,1,3}; +const uint8_t GaitLegOffset[NUM_LEGS] = { + 0,2,1,3}; /* Order (number) - 0(0) 2(1) - \\ // -5(5)-- --4(6) - // \\ - 3(3) 1(2) -*/ + 0(0) 2(1) + \\ // + 5(5)-- --4(6) + // \\ + 3(3) 1(2) + */ int GaitGen2Cycle(int InputArray[NUM_LEGS][3],int LegNumber, int Period, int Cycle,int Xtrans, int Ytrans,int Ztrans, float ZTurn, int NumLegsPeriod = NUM_LEGS){//translation for gait2 float Generator0 = (Cycle-((GaitLegOffset[LegNumber]*Period)/NumLegsPeriod))%Period; //generates cyclic values offeset for leg number. - float Modifiers[4] = {0,0,0,0}; - + float Modifiers[4] = { + 0,0,0,0 }; + if (Generator0<(Period/NumLegsPeriod)){//lifted leg segment float Generator1 = ((2.0*(float)NumLegsPeriod/Period)*(Generator0-(Period/(2.0*(float)NumLegsPeriod))));//Periodic function that goes from -1 to 1 in 1/NumLegs of period if(!(LegNumber%2)){//Invert legs 1 and 3 @@ -22,7 +25,7 @@ int GaitGen2Cycle(int InputArray[NUM_LEGS][3],int LegNumber, int Period, int Cyc Modifiers[0] = Xtrans*Generator1; Modifiers[1] = Ytrans*Generator1; Modifiers[2] = Ztrans; - Modifiers[3] = ((float)ZTurn * 3.14 / 4.0 *Generator1); + Modifiers[3] = ((float)ZTurn * PI / 4.0 *Generator1); } else{//Ground Leg Segment float Generator1 = ((2.0*(float)NumLegsPeriod/Period)*((-Generator0/(NumLegsPeriod-1))+(Period/(2.0*((float)NumLegsPeriod-1)))));//Periodic function that goes from 1 to -1 in NumLegs-1/NumLegs of period @@ -32,67 +35,46 @@ int GaitGen2Cycle(int InputArray[NUM_LEGS][3],int LegNumber, int Period, int Cyc Modifiers[0] = Xtrans*Generator1; Modifiers[1] = Ytrans*Generator1; Modifiers[2] = -Ztrans; - Modifiers[3] = (ZTurn * 3.14 / 4 *Generator1); + Modifiers[3] = (ZTurn * PI / 4 *Generator1); } - + int Output1[3];//Translate - + Output1[0] = InputArray[LegNumber][0] + Modifiers[0];//X Output1[1] = InputArray[LegNumber][1] + Modifiers[1];//Y Output1[2] = InputArray[LegNumber][2] + Modifiers[2];//Z - + int Output2[3];//Rotate - + Output2[0] = (Output1[0]*cosf(Modifiers[3])-Output1[1]*sinf(Modifiers[3])); Output2[1] = (Output1[0]*sinf(Modifiers[3])+Output1[1]*cosf(Modifiers[3])); Output2[2] = Output1[2]; - + //Update input - + InputArray[LegNumber][0] = Output2[0]; InputArray[LegNumber][1] = Output2[1]; InputArray[LegNumber][2] = Output2[2]; } void GaitGen2(int Period, int Cycle, int Xtrans, int Ytrans, int Ztrans, float ZTurn){ - //Sale the inputs - Xtrans =map(Xtrans,-127,127,-40,40); - Ytrans =map(Ytrans,-127,127,-40,40); - Ztrans =map(Ztrans,-127,127,0,20); - ZTurn =-map(ZTurn,-127,127,-50,50)/100; - + //Sale the inputs + Xtrans =map(Xtrans,-127,127,-40,40); + Ytrans =map(Ytrans,-127,127,-40,40); + Ztrans =map(Ztrans,-127,127,0,20); + ZTurn =-map(ZTurn,-127,127,-50,50)/100; + //First pass, sets base position - int Output0[NUM_LEGS][3] = { - { //Leg 0 - (Leg0InitX),//X (Output[0][0]) - (Leg0InitY),//Y (Output[0][1]) - (Leg0InitZ) //Z (Output[0][2]) - - } - , - { //Leg 1 - (Leg1InitX),//X - (Leg1InitY),//Y - (Leg1InitZ) //Z - } - , - { //Leg 2 - (Leg2InitX),//X - (Leg2InitY),//Y - (Leg2InitZ) //Z - } - , - { //Leg 3 - (Leg3InitX),//X - (Leg3InitY),//Y - (Leg3InitZ) //Z - } - , - }; - - for(int i = 0; i DXLServoLimits[i][j][0])&&(ServoPos < DXLServoLimits[i][j][1])){// Servo is within limits ServoWrite(DXLServoLimits[i][j][2],ServoPos); diff --git a/HexapodCodeV3/h_PWMservoDriver.ino b/HexapodCodeV3/h_PWMservoDriver.ino index e1eb2d4..d8af124 100644 --- a/HexapodCodeV3/h_PWMservoDriver.ino +++ b/HexapodCodeV3/h_PWMservoDriver.ino @@ -1,3 +1,4 @@ +//pwmservo // provide control for PWM servos via ADAFRUIT pwm servo driver https://www.adafruit.com/product/815 //Code is based off the library provided and directly uses code diff --git a/HexapodCodeV3/i_inputs.ino b/HexapodCodeV3/i_inputs.ino index fdacad6..6ea383b 100644 --- a/HexapodCodeV3/i_inputs.ino +++ b/HexapodCodeV3/i_inputs.ino @@ -1,4 +1,4 @@ - +//inputs //Defines for Reading Commander #define ButtonRight1 0 #define ButtonRight2 1 @@ -170,8 +170,9 @@ void GetInputs(){ } - switch (InputExtend1 >> 4) { + switch ((InputExtend1 >> 4) & 0x0f) { case 0://Reset Exended byte values + ExtMode0Reset(); break; case 1://Do nothing break; @@ -204,8 +205,8 @@ void GetInputs(){ } } -void ExtMode1Reset(){ - +void ExtMode0Reset(){ + MoveMode = MOVE_MODE_WALK_PERIODIC; } void ExtMode5GaitMode(uint8_t InputGaitMode){ @@ -216,7 +217,7 @@ void ExtMode5GaitMode(uint8_t InputGaitMode){ case 1://Crawl Gait MoveMode = MOVE_MODE_CRAWL_PERIODIC; break; - case 2://Leg Place + case 2://Swerve Mode MoveMode = MOVE_MODE_SWERVE; break; case 3://Rule Gait diff --git a/HexapodCodeV3/j_ControlReset.ino b/HexapodCodeV3/j_ControlReset.ino index 872524f..7e208d5 100644 --- a/HexapodCodeV3/j_ControlReset.ino +++ b/HexapodCodeV3/j_ControlReset.ino @@ -1,3 +1,4 @@ +//controlreset //Code to reset Gait gen control variables //Stops robot and sets cycle to 5 seconds and sep height to 10 cm void GaitGenControlReset(){ diff --git a/HexapodCodeV3/k_sphericalcoords.ino b/HexapodCodeV3/k_sphericalcoords.ino index c13dd2b..c5d06e2 100644 --- a/HexapodCodeV3/k_sphericalcoords.ino +++ b/HexapodCodeV3/k_sphericalcoords.ino @@ -1,3 +1,4 @@ +//Sphericalcoords //Untested and not added to the code, here for memory and future use /* @@ -15,97 +16,110 @@ */ -void LocalSpherical(float Angles[NUM_LEGS][2]){ - /*float LocalLeg1[2]={Angles[0][0],Angles[0][1]}; - float LocalLeg2[2]={-Angles[1][0],-Angles[1][1]}; - float LocalLeg3[2]={-Angles[2][0],-Angles[2][1]}; - float LocalLeg3[2]={Angles[3][0],Angles[3][1]};*/ - Angles[1][0] *= -1; - Angles[1][1] *= -1; - Angles[2][0] *= -1; - Angles[2][1] *= -1; - - for(int i = 0; i < NUM_LEGS; i++){ - LocalServoGen(i,Angles[i][0],Angles[i][1],LegDynamixels[i][0],LegDynamixels[i][1],LegDynamixels[i][2]); - } +void LocalSpherical(void){//convert from global spherical angles to local spherical angles + //Leg0, no change + LegLocalSpherical[0][0] = LegGlobalSpherical[0][0];//Azimuth + LegLocalSpherical[0][1] = LegGlobalSpherical[0][1];//Elevation + LegLocalSpherical[0][2] = LegGlobalSpherical[0][2];//Bank + //Leg1, no change + LegLocalSpherical[0][0] = LegGlobalSpherical[0][0];//Azimuth + LegLocalSpherical[0][1] = LegGlobalSpherical[0][1];//Elevation + LegLocalSpherical[0][2] = LegGlobalSpherical[0][2];//Bank + //Leg2, Azimuth +180 degrees + LegLocalSpherical[0][0] = LegGlobalSpherical[0][0] + PI;//Azimuth + LegLocalSpherical[0][1] = LegGlobalSpherical[0][1];//Elevation + LegLocalSpherical[0][2] = LegGlobalSpherical[0][2];//Bank + //Leg3, Azimuth +180 degrees + LegLocalSpherical[0][0] = LegGlobalSpherical[0][0] + PI;//Azimuth + LegLocalSpherical[0][1] = LegGlobalSpherical[0][1];//Elevation + LegLocalSpherical[0][2] = LegGlobalSpherical[0][2];//Bank } -void LocalServoGen(int leg,float azimuth, float elevation, float coxa, float femur, float tibia){//Calculates new TibaRot and Tarsus based on local azimuth and elevation - //float IniVect[3] = {cos(coxa)*sin(femur+tibia),sin(coxa)*cos(femur+tibia),cos(femur+tibia)};//vector of the leg(x,y,z) - azimuth -=coxa;//The first rotation is faster this way, no trig compared to a cartesian rotation - float DesVect1[3] = { - 10*cosf(azimuth)*sinf(elevation),10*sinf(azimuth)*sinf(elevation),10*cosf(elevation) };//output vector(x,y,z) - float DesVect2[3]; - //desired vector +void LocalSphericalCalculate(void){//Calculates new TibaRot and Tarsus based on local azimuth and elevation from LegLocalSpherical + //Call after calulating and updating LegDynamixels + for(int leg = 0; leg < NUM_LEGS; leg++){ + float Azimuth = LegLocalSpherical[leg][0]; + float Elevation = LegLocalSpherical[leg][1]; + float Coxa = LegDynamixels[leg][0]; + float Femur = LegDynamixels[leg][1]; + float Tibia = LegDynamixels[leg][2]; + //float IniVect[3] = {cos(coxa)*sin(femur+tibia),sin(coxa)*cos(femur+tibia),cos(femur+tibia)};//vector of the leg(x,y,z) + Azimuth -=Coxa;//The first rotation is faster this way, no trig compared to a cartesian rotation + float DesVect1[3] = { + 10*cosf(Azimuth)*sinf(Elevation),10*sinf(Azimuth)*sinf(Elevation),10*cosf(Elevation) };//output vector(x,y,z) + float DesVect2[3]; + //desired vector + + + float pitch = (HALF_PI + Femur + Tibia); + UserSerial.println(pitch); - float pitch = (1.5708 + femur + tibia); - UserSerial.println(pitch); + float XZCorr[3][3] = {//Matrix to rotate point to be in proper loaction for sphericalcoords + { + cosf(pitch),0,sinf(pitch) } + , + { + 0,1,0 } + , + { + -sinf(pitch),0,cosf(pitch) } + }; - float XZCorr[3][3] = {//Matrix to rotate point to be in proper loaction for sphericalcoords - {cosf(pitch),0,sinf(pitch)}, - {0,1,0}, - {-sinf(pitch),0,cosf(pitch)}}; - - #if defined(UserSerialTransmit) - UserSerial.print("Angle x "); - UserSerial.println(DesVect1[0]); - UserSerial.print("Angle y "); - UserSerial.println(DesVect1[1]); - UserSerial.print("Angle z "); - UserSerial.println(DesVect1[2]); + UserSerial.print("Angle x "); + UserSerial.println(DesVect1[0]); + UserSerial.print("Angle y "); + UserSerial.println(DesVect1[1]); + UserSerial.print("Angle z "); + UserSerial.println(DesVect1[2]); #endif - //Preform the second rotation - DesVect2[0] = XZCorr[0][0]*DesVect1[0] + XZCorr[0][1]*DesVect1[1] + XZCorr[0][2]*DesVect1[2]; - DesVect2[1] = XZCorr[1][0]*DesVect1[0] + XZCorr[1][1]*DesVect1[1] + XZCorr[1][2]*DesVect1[2]; - DesVect2[2] = XZCorr[2][0]*DesVect1[0] + XZCorr[2][1]*DesVect1[1] + XZCorr[2][2]*DesVect1[2]; + //Preform the second rotation + DesVect2[0] = XZCorr[0][0]*DesVect1[0] + XZCorr[0][1]*DesVect1[1] + XZCorr[0][2]*DesVect1[2]; + DesVect2[1] = XZCorr[1][0]*DesVect1[0] + XZCorr[1][1]*DesVect1[1] + XZCorr[1][2]*DesVect1[2]; + DesVect2[2] = XZCorr[2][0]*DesVect1[0] + XZCorr[2][1]*DesVect1[1] + XZCorr[2][2]*DesVect1[2]; + + LegDynamixels[leg][3] = atan2f(DesVect2[1],DesVect2[0]);//Tibia Rotation in Degrees + LegDynamixels[leg][4] = -(acosf(DesVect2[2]/10));//Tarsus + //LegDynamixels[leg][5] = LegLocalSpherical[leg][2];//wheel, Not currently implemented in hardware. + } +} - LegDynamixels[leg][3] = atan2f(DesVect2[1],DesVect2[0]);//Tibia Rotation in Degrees - LegDynamixels[leg][4] = -(acosf(DesVect2[2]/10));//Tarsus -#if defined(UserSerialTransmit) - UserSerial.print("FirstShift x "); - UserSerial.println(DesVect2[0]); - UserSerial.print("FirstShift y "); - UserSerial.println(DesVect2[1]); - UserSerial.print("FirstShift z "); - UserSerial.println(DesVect2[2]); - - UserSerial.print("SecondShift x "); - UserSerial.println(DesVect1[0]); - UserSerial.print("SecondShift y "); - UserSerial.println(DesVect1[1]); - UserSerial.print("SecondShift z "); - UserSerial.println(DesVect1[2]); - - UserSerial.println(""); -#endif +void LegPlaceSixCalculate (void){//place leg in six degrees of freedom based on LegCoordsOut and LegLocalSpherical leg -1 places all legs + //Transform the coordinates for each leg + for(int leg = 0; leg < NUM_LEGS; leg++){ + int Xplace = LegCoordsOut[leg][0]; + int Yplace = LegCoordsOut[leg][1]; + int Zplace = LegCoordsOut[leg][2]; + float Azimuth = LegLocalSpherical[leg][0]; + float Elevation = LegLocalSpherical[leg][1]; + float Bank = LegLocalSpherical[leg][2]; + + //Shift X,Y, and Z coordinates to compinsate for the tarsus + float radius = TARSUS_LENGTH*cosf(Elevation); + Xplace -= (radius*cosf(Azimuth)); + Yplace -=(radius*sinf(Azimuth)); + Zplace -=(TARSUS_LENGTH*sinf(Elevation)); + + LegCoordsOut[leg][0] = Xplace; + LegCoordsOut[leg][1] = Yplace; + LegCoordsOut[leg][2] = Zplace; + } - //other stuff + //calculate the angles of the first 3 servos + LegCalculate(); + //calculate the angles of the servos for spherical coordinates after calulating first 3 servos + LocalSphericalCalculate(); } -void LegPlace (uint8_t leg, int Xplace, int Yplace, int Zplace, int Xrot, int Yrot, int Zrot){ - int Output1[NUM_LEGS][3] = {//inital places - { //Leg 0 - Leg0InitX, Leg0InitY, Leg0InitZ}, - { //Leg 1 - Leg1InitX, Leg1InitY, Leg1InitZ}, - { //Leg 2 - Leg2InitX, Leg2InitY, Leg2InitZ}, - { //Leg 3 - Leg3InitX, Leg3InitY, Leg3InitZ} - }; - - -} diff --git a/HexapodCodeV3/l_swerveSteer.ino b/HexapodCodeV3/l_swerveSteer.ino index 4c21dc5..eafd456 100644 --- a/HexapodCodeV3/l_swerveSteer.ino +++ b/HexapodCodeV3/l_swerveSteer.ino @@ -1,21 +1,23 @@ - +//Swervesteer const int SWERVEBASEWIDTH = 300; const int SWERVEBASELENGTH = 300; -void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth = 0, int WheelLength = 0){ - +void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth, int WheelLength){ + //map inputs Xmove = float(map(Xmove,-127,127,-100,100))/100.0; Ymove = float(map(Ymove,-127,127,-100,100))/100.0; - Zrot = float(map(Zrot,-127,127,314,-314))/100.0;//zrot is in radians and counterclockwise is positive + Zrot = float(map(Zrot,-127,127,314,-314))/100.0;//zrot is in radians from +x and counterclockwise is positive WheelWidth = map(WheelWidth,-127,127,-100,100)+SWERVEBASEWIDTH; WheelLength = map(WheelWidth,-127,127,-100,100)+SWERVEBASELENGTH; + float WheelVelocity = 0.001; //value unknown + //calcualte the leg positions and output them int LegPos[NUM_LEGS][3] = { - {-WheelWidth/2,WheelLength/2,Leg0InitZ}, - {WheelWidth/2,WheelLength/2,Leg1InitZ}, - {WheelWidth/2,-WheelLength/2,Leg2InitZ}, - {-WheelWidth/2,-WheelLength/2,Leg3InitZ}}; + {-WheelWidth/2,WheelLength/2,InitialPositions[0][2]}, + {WheelWidth/2,WheelLength/2,InitialPositions[1][2]}, + {WheelWidth/2,-WheelLength/2,InitialPositions[2][2]}, + {-WheelWidth/2,-WheelLength/2,InitialPositions[3][2]}}; for(int i = 0; i < NUM_LEGS; i++){ for(int j = 0; j < 3; j++){ GaitGenOut[i][j] = LegPos[i][j]; @@ -38,12 +40,15 @@ void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth = 0, int W {atan2(ModeA,ModeD),0,sqrt(ModeA*ModeA+ModeD*ModeD)},//BL {atan2(ModeB,ModeD),0,sqrt(ModeB*ModeB+ModeD*ModeD)}};//FL + //duplicate values for scaling float LegWheelSpherical2[4][3]; for(int i = 0; i < 4; i++){ for(int j = 0; j < 3; j++){ LegWheelSpherical2[i][j] = LegWheelSpherical1[i][j]; } } + + //Scale values so largest has a max of 1 if (LegWheelSpherical1[0][2] > 1){ for(int i = 0; i < 4; i++){ @@ -66,5 +71,10 @@ void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth = 0, int W } } - //Set the legs and motors + //Set the spherical part + for(int i = 0; i < 3; i++){ + LegGlobalSpherical[i][0] = LegWheelSpherical1[1][0]; + LegGlobalSpherical[i][1] = LegWheelSpherical1[1][1]; + LegGlobalSpherical[i][2] += LegWheelSpherical1[1][2]*WheelVelocity; + } } diff --git a/HexapodCodeV3/m_Display.ino b/HexapodCodeV3/m_Display.ino index 2a91e1c..5461907 100644 --- a/HexapodCodeV3/m_Display.ino +++ b/HexapodCodeV3/m_Display.ino @@ -1,3 +1,4 @@ +//display /* provide control for Display Requires changing Adafruit_SSD1306.cpp to change from wire.h to i2c_t3.h and commenting out compiler sections starting with "#ifndef __SAM3X8E__" diff --git a/HexapodCodeV3/n_Gimbal.ino b/HexapodCodeV3/n_Gimbal.ino index 40826f9..15ca844 100644 --- a/HexapodCodeV3/n_Gimbal.ino +++ b/HexapodCodeV3/n_Gimbal.ino @@ -1,3 +1,4 @@ +//gimbal //Functions to control the Gimbal using PPM diff --git a/HexapodCodeV4/HexapodCodeV4.ino b/HexapodCodeV4/HexapodCodeV4.ino new file mode 100644 index 0000000..0715dcb --- /dev/null +++ b/HexapodCodeV4/HexapodCodeV4.ino @@ -0,0 +1,91 @@ +/* + Code by Alchemsit_Arthur for a teensy 3.6 based quad. Can also be used on a hexapod. + V4 rewrite to clean up structure to axproximately align with google's C++ style guide https://google.github.io/styleguide/cppguide.html + presetup, setup, loop, gait1, gait2, bodymod, IK, servocontrol, PWMservoDriver, inputs, ControlReset have been updated + + uses globals for leg transitions. Also uses floats. + Y Axis is to the Front + X Axis is to the right + Z Axis is to the top + + +Y + ^ Front + | + | 0 ^ 1 + | \\| |// + | 4===| |===5 + | //| |\\ + | 3 v 2 + o---------->+X Right + / + / + +z + Top + + Leg + + body -> Coxa -> tibia -> femur -> femurRot -> tharsus + + Controls + Takes custom packet protocol + 12 bytes + 1 - 0xFF + 2 - 0xAA + 3 - X, 3 to 253, centered at 128 + 4 - Y, 3 to 253, centered at 128 + 5 - Z, 3 to 253, centered at 128 + 6 - Rx, 3 to 253, centered at 128 + 7 - Ry, 3 to 253, centered at 128 + 8 - Rz, 3 to 253, centered at 128 + 9 - Buttons, each button is 1 bit, from lsb to msb; R1,R2,R3,L1,L2,L3,RT,LT + 10- Extended byte1, + 11- Extended byte2, + 12 - checksum, 255-((b3+b4+b5+b6+b7+b8+b9+b10+b11)%256) + Extended Byte + bits 4-7 are command, bits 0-3 is data (cccc dddd) + Commands Data other + 0 -Reset - NA - reset any set extended byte values + 1 -Nothing - NA - no function, does not reset any values + 2 -Ext1 Follows - Ext1 + 3 -Ext2 Follows - Ext2 + 4 -Leg Place mode - Selected leg, numbers are as above - X->x,Y->y,Z->Z,Rz->WheelAzimuth,Rx->WheelElevation, Ry to wheel rotation + 5 -GaitMode - Selected Gait - 0:WalkPeriodic,1:CrawlPeriodic,2:SwerveSteer,3:RuleGait + 6 - + 7 - + 8 - + 9 - + 10- + 11- + 12- + 13- + 14- + 15- + Gait Select + Data + 0 - Default Gait (periodic gait) + 1 - Crawl Gait (periodic gait) + 2 - Swerve Mode (Swerve Steering) + 3 - Rule based gait (Rule based) + + Status Lights + RGB User Light + Blue - light fades to show running + Green - indicates valid input packets + Red - indicates an error + Board Lights from edge to center + Blue - XBee Assoc + Yellow - XBee RSSI + Green - XBee On + Red - Error + Green - Running Blink + */ + + + + + + + + + + diff --git a/HexapodCodeV4/a_presetup.ino b/HexapodCodeV4/a_presetup.ino new file mode 100644 index 0000000..2e6431e --- /dev/null +++ b/HexapodCodeV4/a_presetup.ino @@ -0,0 +1,249 @@ +//presetup +//Enable return data for userSerial +#define USER_SERIAL_TRANSMIT + +//Enable OLED use +#define OLED_ENABLE + +//Enable Gimbal use +//#define GIMBAL_ENABLE + +//Serial Ports +#define USER_SERIAL Serial +#define DXL_SERIAL Serial1 +#define USB_SERIAL Serial +#define COMMAND_SERIAL Serial2 + + +#if defined(__AVR__) +#error "Sorry, This code requires Teensy 3.x" +#endif + +//Libraries +#include //KurtE's bioloid library https://github.com/KurtE/BioloidSerial +#include //KurtE's bioloid library https://github.com/KurtE/BioloidSerial + +#include //enable use of cosf and sinf to use FPU + +#include //Teensy3.x I2C library to use hardware i2c + +#if defined(GIMBAL_ENABLE)//Gimbal Library +#include +#endif // end gimbal library + +#include +//#include //Conflicts with i2c_t3 + +#if defined(OLED_ENABLE) //Libraries for OLED +#include +#include +#endif //End OLED libraries + + + + +//Error LED +#define LED_RUNNING 12 //red led by Xbee +#define LED_ERROR 24 //yellow led by Xbee + +//UI LEDs +#define LED_RGB_ON HIGH //On state for LEDs +#define LED_RGB_OFF LOW //Off state for LEDS +#define LED_RGB_RED 23 //red LED on indicator button +#define LED_RGB_GRE 22 //green LED on indicator button +#define LED_RGB_BLU 21 //blue LED on indicator button +#define BUTTON_RGB_PIN 20 //the button on the indicator button +#define BUTTON_RGB_MODE INPUT_PULLUP //pinmode for the indicator button pin + + +#define ServoI2C Wire1 + +#define NUM_LEGS 4 +#define NUM_SERVOS_PER_LEG 5 + +//Move Mode -------------------------------------- +#define MOVE_MODE_WALK_PERIODIC 0 +#define MOVE_MODE_CRAWL_PERIODIC 1 +#define MOVE_MODE_SWERVE 2 +#define MOVE_MODE_LEG_PLACE 3 +#define MOVE_MODE_WALK_RULE 4 //Not written yet + +uint8_t move_mode = MOVE_MODE_WALK_PERIODIC; +//uint8_t MoveMode = MOVE_MODE_CRAWL_PERIODIC; + +//Error States +uint8_t error_state = 0;//Error state is the current error +#define ERROR_DXL 0 //an error with dynamixel communications, indicated by bit 0 of ErrorState +#define ERROR_CMD 1 //more than set amount of time passed between command packets, bit 1 of ErrorState + + + + +//Variable used to check servo states +int servo_check_val[NUM_LEGS][NUM_SERVOS_PER_LEG] = { + 0}; + + +//Variables used for command Data in Inputs +int8_t input_x; +int8_t input_y; +int8_t input_z; +int8_t input_rx; +int8_t input_ry; +int8_t input_rz; +uint8_t input_buttons; +uint8_t input_extend1; +uint8_t input_extend2; + + + +//GaitGenControlVars +int gait_period = 2000; //walk period +int gait_move_x = 0; +int gait_move_y = 0; +int gait_step_height = 0; +float gait_move_z_rot = 0; +int gait_body_x = 0; +int gait_body_y = 0; +int gait_body_z = 0; +float gait_body_pitch = 0; +float gait_body_roll = 0; +float gait_body_yaw = 0; + + +//Gimbal definitions +#if defined GIMBAL_ENABLE +PulsePositionOutput GimbalOut; +#define GIMBAL_PPM_PIN 5 +#endif +int cam_pan = 0;//Unused by GG +int cam_tilt = 0;//Unused by GG +int gimbal_mode = 0; + + +//OLED definitions +#if defined(OLED_ENABLE)//Open OLED Definitions +#define OLED_RESET 27 +Adafruit_SSD1306 display(OLED_RESET); + +#if (SSD1306_LCDHEIGHT != 32) +#error("Height incorrect, please fix Adafruit_SSD1306.h!"); +#endif +//My OLED definitions +#define OLED_HEIGHT 32 +#define OLED_WIDTH 128 +#define OLED_I2C Wire + +#endif//End OLED Definitions + +//Defines for BioloidSerial +#define NUM_SERVOS 16 +BioloidControllerEx bioloid = BioloidControllerEx(); + +//Limits and id for each motor in the design in order min, max, ID +//ServoLimits +const int kDXLServoLimits[NUM_LEGS][NUM_SERVOS_PER_LEG][3] = {//min,max,ID + {//Leg0 + {538,759,2}, //Leg0 Coxa + {183,813,3}, //Leg0 Femur + {39,941,4}, //Leg0 Tibia + {205,820,5}, //Leg0 TibiaRotate + {205,820,6} //Leg0 Tarsus + }, + {//Leg1 + {280,469,7}, //Leg1 Coxa + {193,809,8}, //Leg1 Femur + {56,930,9}, //Leg1 Tibia + {211,814,10},//Leg1 TibiaRotate + {211,814,11} //Leg1 Tarsus + }, + {//Leg2 + {543,756,12},//Leg2 Coxa + {186,814,13},//Leg2 Femur + {44,926,14}, //Leg2 Tibia + {188,817,15},//Leg2 TibiaRotate + {188,817,16} //Leg2 Tarsus + }, + {//Leg3 + {262,468,17},//Leg3 Coxa + {182,824,18},//Leg3 Femur + {69,941,19}, //Leg3 Tibia + {208,812,20},//Leg3 TibiaRotate + {211,814,21} //Leg3 Tarsus + } +}; + +#define PWM_SERVOS_MIN 150 //min pulse out of 4096 +#define PWM_SERVOS_MAX 600 //max pulse out of 4096 + +#define NUM_PWM_SERVOS 6 +const int kPWMServoLimits[NUM_PWM_SERVOS][3] = {//order min,max,id + {0,180,0},//Servo0 + {0,180,1},//Servo1 + {0,180,2},//Servo2 + {0,180,3},//Servo3 + {0,180,4},//Servo4 + {0,180,5} //Servo5 +}; + +float leg_dynamixels[NUM_LEGS][NUM_SERVOS_PER_LEG]{ + 0}; + +float pwm_servo_pos[NUM_PWM_SERVOS]={ + 0,0,0,0,0,0};//not used on the robot + +//Initial Positions of legs, used in GaitGen +//Leg 0 (front left) +#define LEG0_INIT_X -120 +#define LEG0_INIT_Y 120 +#define LEG0_INIT_Z -100 +//Leg1 (front right) +#define LEG1_INIT_X 120 +#define LEG1_INIT_Y 120 +#define LEG1_INIT_Z -100 +//Leg2 (back right) +#define LEG2_INIT_X 120 +#define LEG2_INIT_Y -120 +#define LEG2_INIT_Z -100 +//Leg3 (back left +#define LEG3_INIT_X -120 +#define LEG3_INIT_Y -120 +#define LEG3_INIT_Z -100 + +const int kInitialPositions [NUM_LEGS] [3] = {//x,y,z + {LEG0_INIT_X, LEG0_INIT_Y, LEG0_INIT_Z},//Leg0 + {LEG1_INIT_X, LEG1_INIT_Y, LEG1_INIT_Z},//Leg1 + {LEG2_INIT_X, LEG2_INIT_Y, LEG2_INIT_Z},//Leg2 + {LEG3_INIT_X, LEG3_INIT_Y, LEG3_INIT_Z} //Leg3 +}; + + +//Dimensions used in leg calculations +#define COXA_LENGTH 50 +#define FEMUR_LENGTH 93 +#define TIBIA_LENGTH 97 +#define TARSUS_LENGTH 25 + +//Dimensions used to convert to local leg coords +#define COG_X 52 // distance that the legs are in left and right of the COG (center of gravity) +#define COG_Y 65 // distance that the legs are in front and behind the COG + +//Period of the status LED +unsigned int led_running_period = 2000; + +//Variable used to output from functions +uint8_t current_voltage = 0; //battery voltage in tenths +unsigned long last_voltage_read = 0; +unsigned long time_1; +unsigned long last_cycle_start = 0; +unsigned int last_cycle_time = 0; +int gait_gen_out[NUM_LEGS][3]; // For GaitGen [leg] [x,y,z] +int body_mod_out[NUM_LEGS][3]; // for BodyMod [leg] [x,y,z] +int leg_coords_out[NUM_LEGS][3];// For LegCoordsOut, [leg] [x,y,z] +//float LegCalculateOut[NUM_LEGS][NUM_SERVOS_PER_LEG];// For LegCalculateOut, [leg] [servo] //not used +float leg_global_spherical[NUM_LEGS][3]; //For GaitGen/SwerveSteer, [leg] [Azimuth, Elevation, Bank] +float leg_local_spherical[NUM_LEGS][3]; //For LocalSpherical, [leg] [Azimuth, Elevation, Bank] + + + + diff --git a/HexapodCodeV4/b_setup.ino b/HexapodCodeV4/b_setup.ino new file mode 100644 index 0000000..37436f5 --- /dev/null +++ b/HexapodCodeV4/b_setup.ino @@ -0,0 +1,66 @@ +//setup +void setup(){ + + + //Open Serial Ports + USER_SERIAL.begin(38400); + COMMAND_SERIAL.begin(38400); + +#if defined(GIMBAL_ENABLE)//Gimbal Enable + GimbalOut.begin(GIMBAL_PPM_PIN); +#endif//end gimbal enable + +#if defined(OLED_ENABLE)//OLED Enable + display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) + display.display(); +#endif//End OLED Enable + delay(2000); + +#if defined(USER_SERIAL_TRANSMIT) + USER_SERIAL.println("Started"); +#endif + + //Set pinmode for LED pins + pinMode(BUTTON_RGB_PIN,BUTTON_RGB_MODE); + pinMode(LED_RUNNING,OUTPUT); + pinMode(LED_ERROR,OUTPUT); + pinMode(LED_RGB_RED,OUTPUT); + pinMode(LED_RGB_GRE,OUTPUT); + pinMode(LED_RGB_BLU,OUTPUT); + + + //Setup Dynamixels + bioloid.begin(1000000,&DXL_SERIAL,-1); + bioloid.poseSize = NUM_SERVOS; + //PWMI2C_Begin(); + GaitGenControlReset(); + + //Verify All servos are in place + for (int i = 0; i < NUM_LEGS; i++){ + for (int j = 0; j < NUM_SERVOS_PER_LEG; j++){ + if (ServoPosQuerry(kDXLServoLimits[i][j][2])== -1){ + servo_check_val[i][j] = 1; +#if defined(USER_SERIAL_TRANSMIT) + USER_SERIAL.print("Servo "); + USER_SERIAL.print(kDXLServoLimits[i][j][2]); + USER_SERIAL.println(" is missing"); +#endif + } + else{ +#if defined(USER_SERIAL_TRANSMIT) + USER_SERIAL.print("Servo "); + USER_SERIAL.print(kDXLServoLimits[i][j][2]); + USER_SERIAL.print(" is at "); + USER_SERIAL.println(ServoPosQuerry(kDXLServoLimits[i][j][2])); +#endif + } + } + } + + + //Test if servo has reset to ID 1 + if (ServoPosQuerry(1)!= -1){ + USER_SERIAL.println("Servo 1 Detected, Error?"); + } +} + diff --git a/HexapodCodeV4/c_loop.ino b/HexapodCodeV4/c_loop.ino new file mode 100644 index 0000000..858bf19 --- /dev/null +++ b/HexapodCodeV4/c_loop.ino @@ -0,0 +1,113 @@ +//loop +void loop() { + time_1 = millis(); + last_cycle_time = time_1 - last_cycle_start; + last_cycle_start = time_1; + + if ((time_1 - last_voltage_read) > 500){ //read the voltage of servo two every half second + current_voltage = ax12GetRegister(2,AX_PRESENT_VOLTAGE,1); + last_voltage_read = time_1; + } + + + //Display the time on the display + MainDisplay(); +#if defined(OLED_ENABLE) + display.display(); +#endif + + + + GetInputs(); + switch (move_mode){ + case MOVE_MODE_WALK_PERIODIC: + GaitGen1(gait_period, time_1, gait_move_x, gait_move_y, gait_step_height, gait_move_z_rot); + BodyMod(gait_body_x, gait_body_y, gait_body_z, gait_body_pitch, gait_body_roll, gait_body_yaw); + LegCoords(); + LegCalculate(); + break; + case MOVE_MODE_CRAWL_PERIODIC: + GaitGen2(gait_period, time_1, gait_move_x, gait_move_y, gait_step_height, gait_move_z_rot); + BodyMod(gait_body_x, gait_body_y, gait_body_z, gait_body_pitch, gait_body_roll, gait_body_yaw); + LegCoords(); + LegCalculate(); + break; + case MOVE_MODE_SWERVE: + SwerveSteer(gait_move_x, gait_move_y, gait_move_z_rot, gait_step_height, gait_step_height); + BodyMod(gait_body_x, gait_body_y, gait_body_z, gait_body_pitch, gait_body_roll, gait_body_yaw); + LegCoords(); + LocalSpherical(); + LegPlaceSixCalculate(); + break; + } + + + //sphericalCoords goes here +#if defined(GIMBAL_ENABLE) + GimbalControl(cam_pan, cam_tilt, gimbal_mode); +#endif + + DXLServoMap();//Output motor values + //PWMServoMap(); + + + //Debug Print Statements +#if defined(USER_SERIAL_TRANSMIT) + Serial.println(time_1); + for (int i = 0; i < 4; i++){ + USER_SERIAL.println("------"); + USER_SERIAL.print("Coxa = "); + USER_SERIAL.println(leg_dynamixels[i][0]); + USER_SERIAL.print("Femur = "); + USER_SERIAL.println(leg_dynamixels[i][1]); + USER_SERIAL.print("Tibia = "); + USER_SERIAL.println(leg_dynamixels[i][2]); + USER_SERIAL.print("Tibia2 = "); + USER_SERIAL.println(leg_dynamixels[i][3]); + USER_SERIAL.print("Tharsus = "); + USER_SERIAL.println(leg_dynamixels[i][4]); + USER_SERIAL.print("\n"); + } +#endif + + //Blink the running LED + //uint8_t led_state= abs((time_1%led_running_period)/(led_running_period/512)-255); + if (time_1%2000<1000){ + digitalWrite(LED_RUNNING,HIGH); + digitalWrite(LED_RGB_BLU,HIGH); + digitalWrite(LED_RGB_RED,HIGH); + digitalWrite(LED_RGB_GRE,HIGH); + } + else{ + digitalWrite(LED_RUNNING,LOW); + digitalWrite(LED_RGB_BLU,LOW); + digitalWrite(LED_RGB_RED,LOW); + digitalWrite(LED_RGB_GRE,LOW); + } + + //Handle error LED patterns + if(error_state & B00000001){//DXL Error + if(time_1%500<150){ + digitalWrite(LED_ERROR,HIGH); + } + else{ + digitalWrite(LED_ERROR,LOW); + } + } + if(error_state & B00000010){//CMD error + if(time_1%500<250){ + digitalWrite(LED_ERROR,HIGH); + } + else{ + digitalWrite(LED_ERROR,LOW); + } + } + + //Fade the Status LED + delay(50); + +} + + + + diff --git a/HexapodCodeV4/d0_gait1.ino b/HexapodCodeV4/d0_gait1.ino new file mode 100644 index 0000000..30c47ed --- /dev/null +++ b/HexapodCodeV4/d0_gait1.ino @@ -0,0 +1,91 @@ +//gait1 +//This code covers the basic Gait Generator it moves diagonal pairs of legs on a quad +void GaitGen1(int period, int cycle, int translate_x, int translate_y, int translate_z, float turn_z){ + //Sale the inputs + translate_x =map(translate_x,-127,127,-40,40); + translate_y =map(translate_y,-127,127,-40,40); + translate_z =map(translate_z,-127,127,0,20); + turn_z =-map(turn_z,-127,127,50,-50)/100; + + float generator1 = ((4.0 *(abs((cycle%period)-(period/2))-(period/4)))/period); + int generator_z = (-1*((abs((cycle%period)-(period/2)))/((cycle%period)-(period/2)))); + float theta1 = (turn_z * PI / 4 *generator1); + + //Type1 and Type two are added to legs to produce movement + float offsets1[3] = {(translate_x*generator1),(translate_y*generator1),(translate_z*generator_z)}; + float offsets2[3] = {(translate_x*generator1),(translate_y*generator1),(-translate_z*generator_z)}; + + + //First pass does gait generation for translation + int output1[NUM_LEGS][3] = { + { //Leg 0 + (kInitialPositions[0][0] + offsets1[0]),//X (output[0][0]) + (kInitialPositions[0][1] + offsets1[1]),//Y (output[0][1]) + (kInitialPositions[0][2] + offsets1[2]) //Z (output[0][2]) + },//end Leg 0 + { //Leg 1 + (kInitialPositions[1][0] + offsets2[0]),//X + (kInitialPositions[1][1] + offsets2[1]),//Y + (kInitialPositions[1][2] + offsets2[2]) //Z + },//end Leg 1 + { //Leg 2 + (kInitialPositions[2][0] + offsets1[0]),//X + (kInitialPositions[2][1] + offsets1[1]),//Y + (kInitialPositions[2][2] + offsets1[2]) //Z + },//end Leg 2 + { //Leg 3 + (kInitialPositions[3][0] + offsets2[0]),//X + (kInitialPositions[3][1] + offsets2[1]),//Y + (kInitialPositions[3][2] + offsets2[2]) //Z + },//end Leg 3 + }; + + + //Second pass does gait rotate in place + int output2[NUM_LEGS][3]; + output2[0][0] = (output1[0][0]*cosf(theta1)-output1[0][1]*sinf(theta1)); + output2[0][1] = (output1[0][0]*sinf(theta1)+output1[0][1]*cosf(theta1)); + output2[0][2] = output1[0][2]; + + output2[1][0] = (output1[1][0]*cosf(-theta1)-output1[1][1]*sinf(-theta1)); + output2[1][1] = (output1[1][0]*sinf(-theta1)+output1[1][1]*cosf(-theta1)); + output2[1][2] = output1[1][2]; + + output2[2][0] = (output1[2][0]*cosf(theta1)-output1[2][1]*sinf(theta1)); + output2[2][1] = (output1[2][0]*sinf(theta1)+output1[2][1]*cosf(theta1)); + output2[2][2] = output1[2][2]; + + output2[3][0] = (output1[3][0]*cosf(-theta1)-output1[3][1]*sinf(-theta1)); + output2[3][1] = (output1[3][0]*sinf(-theta1)+output1[3][1]*cosf(-theta1)); + output2[3][2] = output1[3][2]; + + //Update GaitGenOut + for(int i = 0; i < NUM_LEGS; i++){ + for(int j = 0; j < 3; j++){ + gait_gen_out[i][j] = output2[i][j]; + } + } + + //Debug Print + //UserSerial.print("Generator 1 = "); + //UserSerial.println(Generator1); + //UserSerial.print("Generator 2 = "); + //UserSerial.println(Generator2); + //UserSerial.print("Generator Z = "); + //UserSerial.println(GeneratorZ); + //UserSerial.print("Type1 = "); + //UserSerial.print(Type1[0]); + //UserSerial.print(" "); + //UserSerial.print(Type1[1]); + //UserSerial.print(" "); + //UserSerial.println(Type1[2]); + //UserSerial.print("Type2 = "); + //UserSerial.print(Type2[0]); + //UserSerial.print(" "); + //UserSerial.print(Type2[1]); + //UserSerial.print(" "); + //UserSerial.println(Type2[2]); + + + +} diff --git a/HexapodCodeV4/d1_gait2.ino b/HexapodCodeV4/d1_gait2.ino new file mode 100644 index 0000000..fea12f8 --- /dev/null +++ b/HexapodCodeV4/d1_gait2.ino @@ -0,0 +1,89 @@ +//gait2 +//This code covers the crawl Gait Generator +//WIP +const uint8_t kGaitLegOffset[NUM_LEGS] = {0,2,1,3}; +/* + Order (number) + 0(0) 2(1) + \\ // + 5(4)-- --4(5) + // \\ + 3(3) 1(2) + */ + +int GaitGen2Cycle(int input_array[NUM_LEGS][3],int leg_number, int period, int cycle,int translate_x, int translate_y,int translate_z, float turn_z){//translation for gait2 + int num_legs_period = NUM_LEGS; + float generator0 = (cycle-((kGaitLegOffset[leg_number]*period)/num_legs_period))%period; //generates cyclic values offeset for leg number. + float modifiers[4] = { + 0,0,0,0 }; + + if (generator0<(period/num_legs_period)){//lifted leg segment + float generator1 = ((2.0*(float)num_legs_period/period)*(generator0-(period/(2.0*(float)num_legs_period))));//periodic function that goes from -1 to 1 in 1/NumLegs of period + if(!(leg_number%2)){//Invert legs 1 and 3 + generator1 = -generator1; + } + modifiers[0] = translate_x*generator1; + modifiers[1] = translate_y*generator1; + modifiers[2] = translate_z; + modifiers[3] = -((float)turn_z * PI / 4.0 *generator1); + } + else{//Ground Leg Segment + float generator1 = ((2.0*(float)num_legs_period/period)*((-generator0/(num_legs_period-1))+(period/(2.0*((float)num_legs_period-1)))));//periodic function that goes from 1 to -1 in NumLegs-1/NumLegs of period + if(!(leg_number%2)){//Invert legs 1 and 3 + generator1 = -generator1; + } + modifiers[0] = translate_x*generator1; + modifiers[1] = translate_y*generator1; + modifiers[2] = -translate_z; + modifiers[3] = -(turn_z * PI / 4 *generator1); + } + + int Output1[3];//Translate + + Output1[0] = input_array[leg_number][0] + modifiers[0];//X + Output1[1] = input_array[leg_number][1] + modifiers[1];//Y + Output1[2] = input_array[leg_number][2] + modifiers[2];//Z + + int Output2[3];//Rotate + + Output2[0] = (Output1[0]*cosf(modifiers[3])-Output1[1]*sinf(modifiers[3])); + Output2[1] = (Output1[0]*sinf(modifiers[3])+Output1[1]*cosf(modifiers[3])); + Output2[2] = Output1[2]; + + //Update input + + input_array[leg_number][0] = Output2[0]; + input_array[leg_number][1] = Output2[1]; + input_array[leg_number][2] = Output2[2]; +} + +void GaitGen2(int period, int cycle, int translate_x, int translate_y, int translate_z, float turn_z){ + //Sale the inputs + translate_x =map(translate_x,-127,127,-40,40); + translate_y =map(translate_y,-127,127,-40,40); + translate_z =map(translate_z,-127,127,0,20); + turn_z =-map(turn_z,-127,127,-50,50)/100; + + //First pass, sets base position + int output0[NUM_LEGS][3]; + for (int i = 0; i kDXLServoLimits[i][j][0])&&(servo_pos < kDXLServoLimits[i][j][1])){// Servo is within limits + ServoWrite(kDXLServoLimits[i][j][2],servo_pos); + #if defined(USER_SERIAL_TRANSMIT) + USER_SERIAL.print(kDXLServoLimits[i][j][2]); + USER_SERIAL.print("G"); + #endif + } + else{//Servo not in limits + //Do nothing + #if defined(USER_SERIAL_TRANSMIT) + USER_SERIAL.print(kDXLServoLimits[i][j][2]); + USER_SERIAL.print("B"); + USER_SERIAL.print(servo_pos); + USER_SERIAL.print("--"); + #endif + } + //USER_SERIAL.print(ServoLimits[i][j][2]); + //USER_SERIAL.print(" "); + //USER_SERIAL.println(ServoPos); + } + #if defined(USER_SERIAL_TRANSMIT) + USER_SERIAL.println(); + #endif + } +} +//----------------------------------------------------------------- + +//Querry Servo +//Querry's position of a servo, if no response or response times out returns -1 +//#define AX_PRESENT_POSITION_L +int ServoPosQuerry(int servo_id){ + return (ax12GetRegister(servo_id,AX_PRESENT_POSITION_L,2)); +} + + +//----------------------------------------------------------------- +//Write to Servos +void ServoWrite(int servo_id, int target_position){ + SetPosition(servo_id,target_position); + /*USER_SERIAL.print(ServoID); + USER_SERIAL.print(","); + USER_SERIAL.println(Position); + */ + //Code Here +} + + diff --git a/HexapodCodeV4/h_PWMservoDriver.ino b/HexapodCodeV4/h_PWMservoDriver.ino new file mode 100644 index 0000000..6704ccd --- /dev/null +++ b/HexapodCodeV4/h_PWMservoDriver.ino @@ -0,0 +1,117 @@ +//pwmservo +// provide control for PWM servos via ADAFRUIT pwm servo driver https://www.adafruit.com/product/815 +//Code is based off the library provided and directly uses code + +#define PCA9685_SUBADR1 0x2 +#define PCA9685_SUBADR2 0x3 +#define PCA9685_SUBADR3 0x4 + +#define PCA9685_MODE1 0x0 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x6 +#define LED0_ON_H 0x7 +#define LED0_OFF_L 0x8 +#define LED0_OFF_H 0x9 + +#define ALLLED_ON_L 0xFA +#define ALLLED_ON_H 0xFB +#define ALLLED_OFF_L 0xFC +#define ALLLED_OFF_H 0xFD + +void PWMServoMap(){ + for(int i = 0; i < NUM_PWM_SERVOS; i++){ + PWMI2C_setPWM(kPWMServoLimits[i][3],0,constrain(map(constrain(pwm_servo_pos[i],kPWMServoLimits[i][0],kPWMServoLimits[i][1]),0,180,PWM_SERVOS_MIN,PWM_SERVOS_MAX),PWM_SERVOS_MIN,PWM_SERVOS_MAX)); + } +} + +uint8_t _i2caddr = 0x40; + +void PWMI2C_Begin(){ + ServoI2C.begin(I2C_MASTER, 0x00, I2C_PINS_37_38, I2C_PULLUP_EXT, 100000); + PWMI2C_Reset(); + +} + +void PWMI2C_Reset(){ + PWMI2C_write8(PCA9685_MODE1,0x0); +} + +void PWMI2C_setPWMFreq(float freq){ + freq *= 0.9; // Correct for overshoot in the frequency setting (see issue #11). + float prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + uint8_t prescale = floor(prescaleval + 0.5); + uint8_t oldmode = PWMI2C_read8(PCA9685_MODE1); + uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + PWMI2C_write8(PCA9685_MODE1, newmode); // go to sleep + PWMI2C_write8(PCA9685_PRESCALE, prescale); // set the prescaler + PWMI2C_write8(PCA9685_MODE1, oldmode); + delay(5); + PWMI2C_write8(PCA9685_MODE1, oldmode | 0xa1); +} + +void PWMI2C_setPWM(uint8_t num, uint16_t on, uint16_t off) { + //Serial.print("Setting PWM "); Serial.print(num); Serial.print(": "); Serial.print(on); Serial.print("->"); Serial.println(off); + + ServoI2C.beginTransmission(_i2caddr); + ServoI2C.write(LED0_ON_L+4*num); + ServoI2C.write(on); + ServoI2C.write(on>>8); + ServoI2C.write(off); + ServoI2C.write(off>>8); + ServoI2C.endTransmission(); +} + +void PWMI2C_setPin(uint8_t num, uint16_t val, bool invert) +{ + // Clamp value between 0 and 4095 inclusive. + val = min(val, 4095); + if (invert) { + if (val == 0) { + // Special value for signal fully on. + PWMI2C_setPWM(num, 4096, 0); + } + else if (val == 4095) { + // Special value for signal fully off. + PWMI2C_setPWM(num, 0, 4096); + } + else { + PWMI2C_setPWM(num, 0, 4095-val); + } + } + else { + if (val == 4095) { + // Special value for signal fully on. + PWMI2C_setPWM(num, 4096, 0); + } + else if (val == 0) { + // Special value for signal fully off. + PWMI2C_setPWM(num, 0, 4096); + } + else { + PWMI2C_setPWM(num, 0, val); + } + } +} + +uint8_t PWMI2C_read8(uint8_t addr) { + ServoI2C.beginTransmission(_i2caddr); + ServoI2C.write(addr); + ServoI2C.endTransmission(); + + ServoI2C.requestFrom((uint8_t)_i2caddr, (uint8_t)1); + return ServoI2C.read(); +} + +void PWMI2C_write8(uint8_t addr, uint8_t d) { + ServoI2C.beginTransmission(_i2caddr); + ServoI2C.write(addr); + ServoI2C.write(d); + ServoI2C.endTransmission(); +} + + + diff --git a/HexapodCodeV4/i_inputs.ino b/HexapodCodeV4/i_inputs.ino new file mode 100644 index 0000000..84738f0 --- /dev/null +++ b/HexapodCodeV4/i_inputs.ino @@ -0,0 +1,229 @@ +//inputs +//Defines for Reading Commander +#define ButtonRight1 0 +#define ButtonRight2 1 +#define ButtonRight3 2 +#define ButtonLeft4 3 +#define ButtonLeft5 4 +#define ButtonLeft6 5 +#define ButtonRightTop 6 +#define ButtonLeftTop 7 + +//What input is which---------------------------------------- +//Move Mode(for analog)--------------- +#define MOVELR input_x +#define MOVEFB input_y +#define MOVEYAW input_rz +#define MOVEUD input_z +// cam_pan EXT1 (in = 0) = 0 (in > 0) -= 8 +// cam_tilt EXT2 (in = 0) = 0 (in > 0) -= 8 +//Body Mode(for analog)---------------- +#define BODYLR input_x +#define BODYFB input_y +#define BODYUD input_z +#define BODYYAW input_rz +#define BODYPITCH input_rx +#define BODYROLL input_ry +// BODYHEIGHT EXT1 (in = 0) = 0 (in > 0) = 8 +// BODYROLL EXT2 (in = 0) = 0 (in > 0) = 8 +//Buttons(mode independant) +#define ANALOGMODEBIT ButtonRightTop +#define ANALOGHOLDBIT ButtonLeftTop + + + +//reminder of variables for gait generation control +/* + //All variables range from -125 to 125 under normal situations + MoveMode what gait to use --uint_8t + GaitPeriod how long one cycle takes + GaitMoveX how far to step each cycle + GaitMoveY how far to step each cycle + GaitStepHeight how far to lift for each step + GaitMoveZrot how far to rotate each cycle + GaitBodyX how far the body should be translated relative to the legs/direction of travel + GaitBodyY how far the body should be translated relative to the legs/direction of travel + GaitBodyZ how far the body should be translated vertically from the default height + GaitBodyPitch how far the body should be rotated relative to the legs/direction of travel + GaitBodyRoll how far the body should be rotated relative to the legs/direction of travel + GaitBodyYaw how far the body should be rotated relative to the legs/direction of travel + */ + +//Error state information +#define CONTROL_PERIOD 1000 //How long before indicating communications problem +unsigned long LastCommand = 0; + + + +//Variables used to read incoming packets +uint8_t IncomingBuffer[12]; +uint16_t CommandChecksum; + + + + +boolean AnalogInMode = 0; +boolean AnalogHold = 0; +uint8_t Ext1 = 0; +uint8_t Ext2 = 0; + +int ReadPacket(){//Read a packet from CommandSerial + if ((millis() - LastCommand) >= CONTROL_PERIOD){//check for error + bitWrite(error_state, ERROR_CMD, 1); + } + else{ + bitWrite(error_state, ERROR_CMD, 0); + } + + while((COMMAND_SERIAL.available()>0)&&(COMMAND_SERIAL.peek()!=255)){ + COMMAND_SERIAL.read(); + } + + if (COMMAND_SERIAL.available() > 11){ + COMMAND_SERIAL.readBytes(IncomingBuffer, 12); + CommandChecksum = 0; + for(int i = 2;i<12;i++){ + CommandChecksum += IncomingBuffer[i]; + } + CommandChecksum %= 256; + + if(IncomingBuffer[0] != 255){//Invalid Packet + return -1; + } + if(CommandChecksum != 255){//Invalid checksum + +#if defined(UserSerialTransmit) + UserSerial.println("\nChecksumError"); +#endif + return -2; + } + else{//ValidChecksum + input_x = (int8_t)( (int)IncomingBuffer[2]-128 ); + input_y = (int8_t)( (int)IncomingBuffer[3]-128 ); + input_z = (int8_t)( (int)IncomingBuffer[4]-128 ); + input_rx = (int8_t)( (int)IncomingBuffer[5]-128 ); + input_ry = (int8_t)( (int)IncomingBuffer[6]-128 ); + input_rz = (int8_t)( (int)IncomingBuffer[7]-128 ); + input_buttons = (uint8_t)(IncomingBuffer[8]); + input_extend1 = (uint8_t)(IncomingBuffer[9]); + input_extend2 = (uint8_t)(IncomingBuffer[10]); + + LastCommand = millis(); + return 1; + } + } + return 0; +} + + +//ReadCommanderData +void GetInputs(){ + unsigned long InputTimeout = millis(); + while((COMMAND_SERIAL.available()>12)&&((millis()-InputTimeout)<100)){ + ReadPacket(); + //ReadButtons + AnalogInMode = bitRead(input_buttons, ANALOGMODEBIT); + AnalogHold = bitRead(input_buttons, ANALOGHOLDBIT); + //insert other buttons here + + + + + //apply holds + if (AnalogHold == 0){ + gait_step_height = -128; + gait_move_y = 0; + gait_move_x = 0; + gait_move_z_rot = 0; + cam_pan = 0; + cam_tilt = 0; + gait_body_y = 0; + gait_body_x = 0; + gait_body_yaw = 0; + gait_body_pitch = 0; + gait_body_roll = 0; + gait_body_z = 0; + } + //Check Mode (body or move) + if (AnalogInMode == 0){//MoveMode + gait_move_y = MOVELR; + gait_move_x = MOVEFB; + gait_move_z_rot = MOVEYAW; + gait_step_height = MOVEUD; + //cam_pan = map(Ext1,-7,7,-127,127); + //cam_tilt = map(Ext2,-7,7,-127,127); + } + else if (AnalogInMode == 1){//BodyMode + gait_body_y = BODYLR; + gait_body_x = BODYFB; + gait_body_yaw = BODYYAW; + gait_body_pitch = BODYPITCH; + gait_body_z = BODYUD; + gait_body_roll = BODYROLL; + } + + if (bitRead(input_buttons, ButtonRight1)){ + move_mode = MOVE_MODE_WALK_PERIODIC; + } + else if (bitRead(input_buttons, ButtonRight2)){ + move_mode = MOVE_MODE_CRAWL_PERIODIC; + } + + + switch ((input_extend1 >> 4) & 0x0f) { + case 0://Reset Exended byte values + ExtMode0Reset(); + break; + case 1://Do nothing + break; + case 2://Legacy EXT1 + break; + case 3://Legacy EXT2 + break; + case 4://Leg Place Mode + break; + case 5://Gait Mode + ExtMode5GaitMode(input_extend1 & 0x0f); + break; + case 6: + break; + case 7: + break; + case 8: + break; + case 9: + break; + case 10: + break; + case 11: + break; + case 12: + break; + case 13: + break; + } + } +} + +void ExtMode0Reset(){ + move_mode = MOVE_MODE_WALK_PERIODIC; +} + +void ExtMode5GaitMode(uint8_t InputGaitMode){ + switch (InputGaitMode) { + case 0://Default Gait + move_mode = MOVE_MODE_WALK_PERIODIC; + break; + case 1://Crawl Gait + move_mode = MOVE_MODE_CRAWL_PERIODIC; + break; + case 2://Swerve Mode + move_mode = MOVE_MODE_SWERVE; + break; + case 3://Rule Gait + move_mode = MOVE_MODE_WALK_RULE; + break; + } +} + + diff --git a/HexapodCodeV4/j_ControlReset.ino b/HexapodCodeV4/j_ControlReset.ino new file mode 100644 index 0000000..9d42fb1 --- /dev/null +++ b/HexapodCodeV4/j_ControlReset.ino @@ -0,0 +1,16 @@ +//controlreset +//Code to reset Gait gen control variables +//Stops robot and sets cycle to 5 seconds and sep height to 10 cm +void GaitGenControlReset(){ + gait_period = 2000; + gait_move_x = 0; + gait_move_y = 0; + gait_step_height = 10; + gait_move_z_rot = 0; + gait_body_x = 0; + gait_body_y = 0; + gait_body_z = 0; + gait_body_pitch = 0; + gait_body_roll = 0; + gait_body_yaw = 0; +} diff --git a/HexapodCodeV4/k_sphericalcoords.ino b/HexapodCodeV4/k_sphericalcoords.ino new file mode 100644 index 0000000..1bc3200 --- /dev/null +++ b/HexapodCodeV4/k_sphericalcoords.ino @@ -0,0 +1,125 @@ +//Sphericalcoords +//Untested and not added to the code, here for memory and future use + +/* + + +Z + ^ +X + | / + | / + | / + | / + o------------> +Y + + X is to the front of the servo + + + */ + +void LocalSpherical(void){//convert from global spherical angles to local spherical angles + //Leg0, no change + leg_local_spherical[0][0] = leg_global_spherical[0][0];//Azimuth + leg_local_spherical[0][1] = leg_global_spherical[0][1];//Elevation + leg_local_spherical[0][2] = leg_global_spherical[0][2];//Bank + //Leg1, no change + leg_local_spherical[0][0] = leg_global_spherical[0][0];//Azimuth + leg_local_spherical[0][1] = leg_global_spherical[0][1];//Elevation + leg_local_spherical[0][2] = leg_global_spherical[0][2];//Bank + //Leg2, Azimuth +180 degrees + leg_local_spherical[0][0] = leg_global_spherical[0][0] + PI;//Azimuth + leg_local_spherical[0][1] = leg_global_spherical[0][1];//Elevation + leg_local_spherical[0][2] = leg_global_spherical[0][2];//Bank + //Leg3, Azimuth +180 degrees + leg_local_spherical[0][0] = leg_global_spherical[0][0] + PI;//Azimuth + leg_local_spherical[0][1] = leg_global_spherical[0][1];//Elevation + leg_local_spherical[0][2] = leg_global_spherical[0][2];//Bank +} + + + + +void LocalSphericalCalculate(void){//Calculates new TibaRot and Tarsus based on local azimuth and elevation from leg_local_spherical + //Call after calulating and updating leg_dynamixels + for(int leg = 0; leg < NUM_LEGS; leg++){ + float spherical_azimuth = leg_local_spherical[leg][0]; + float spherical_elevation = leg_local_spherical[leg][1]; + float angle_coxa = leg_dynamixels[leg][0]; + float angle_femur = leg_dynamixels[leg][1]; + float angle_tibia = leg_dynamixels[leg][2]; + //float IniVect[3] = {cos(coxa)*sin(femur+tibia),sin(coxa)*cos(femur+tibia),cos(femur+tibia)};//vector of the leg(x,y,z) + spherical_azimuth -=angle_coxa;//The first rotation is faster this way, no trig compared to a cartesian rotation + float des_vect1[3] = { + 10*cosf(spherical_azimuth)*sinf(spherical_elevation),10*sinf(spherical_azimuth)*sinf(spherical_elevation),10*cosf(spherical_elevation) };//output vector(x,y,z) + float des_vect2[3]; + //desired vector + + + float pitch = (HALF_PI + angle_femur + angle_tibia); + USER_SERIAL.println(pitch); + + + float XZCorr[3][3] = {//Matrix to rotate point to be in proper loaction for sphericalcoords + { + cosf(pitch),0,sinf(pitch) } + , + { + 0,1,0 } + , + { + -sinf(pitch),0,cosf(pitch) } + }; + + +#if defined(UserSerialTransmit) + USER_SERIAL.print("Angle x "); + USER_SERIAL.println(des_vect1[0]); + USER_SERIAL.print("Angle y "); + USER_SERIAL.println(des_vect1[1]); + USER_SERIAL.print("Angle z "); + USER_SERIAL.println(des_vect1[2]); +#endif + + + //Preform the second rotation + des_vect2[0] = XZCorr[0][0]*des_vect1[0] + XZCorr[0][1]*des_vect1[1] + XZCorr[0][2]*des_vect1[2]; + des_vect2[1] = XZCorr[1][0]*des_vect1[0] + XZCorr[1][1]*des_vect1[1] + XZCorr[1][2]*des_vect1[2]; + des_vect2[2] = XZCorr[2][0]*des_vect1[0] + XZCorr[2][1]*des_vect1[1] + XZCorr[2][2]*des_vect1[2]; + + leg_dynamixels[leg][3] = atan2f(des_vect2[1],des_vect2[0]);//Tibia Rotation in Degrees + leg_dynamixels[leg][4] = -(acosf(des_vect2[2]/10));//Tarsus + //leg_dynamixels[leg][5] = leg_local_spherical[leg][2];//wheel, Not currently implemented in hardware. + } +} + + +void LegPlaceSixCalculate (void){//place leg in six degrees of freedom based on leg_coords_out and leg_local_spherical leg -1 places all legs + //Transform the coordinates for each leg + for(int leg = 0; leg < NUM_LEGS; leg++){ + int place_x = leg_coords_out[leg][0]; + int place_y = leg_coords_out[leg][1]; + int place_z = leg_coords_out[leg][2]; + float foot_azimuth = leg_local_spherical[leg][0]; + float foot_elevation = leg_local_spherical[leg][1]; + float Bank = leg_local_spherical[leg][2]; + + //Shift X,Y, and Z coordinates to compinsate for the tarsus + float radius = TARSUS_LENGTH*cosf(foot_elevation); + place_x -= (radius*cosf(foot_azimuth)); + place_y -=(radius*sinf(foot_azimuth)); + place_z -=(TARSUS_LENGTH*sinf(foot_elevation)); + + leg_coords_out[leg][0] = place_x; + leg_coords_out[leg][1] = place_y; + leg_coords_out[leg][2] = place_z; + } + + //calculate the angles of the first 3 servos + LegCalculate(); + //calculate the angles of the servos for spherical coordinates after calulating first 3 servos + LocalSphericalCalculate(); +} + + + + + diff --git a/HexapodCodeV4/l_swerveSteer.ino b/HexapodCodeV4/l_swerveSteer.ino new file mode 100644 index 0000000..5b034b7 --- /dev/null +++ b/HexapodCodeV4/l_swerveSteer.ino @@ -0,0 +1,80 @@ +//Swervesteer +const int SWERVEBASEWIDTH = 300; +const int SWERVEBASELENGTH = 300; + +void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth, int WheelLength){ + //map inputs + Xmove = float(map(Xmove,-127,127,-100,100))/100.0; + Ymove = float(map(Ymove,-127,127,-100,100))/100.0; + Zrot = float(map(Zrot,-127,127,314,-314))/100.0;//zrot is in radians from +x and counterclockwise is positive + WheelWidth = map(WheelWidth,-127,127,-100,100)+SWERVEBASEWIDTH; + WheelLength = map(WheelWidth,-127,127,-100,100)+SWERVEBASELENGTH; + + float WheelVelocity = 0.001; //value unknown + + //calcualte the leg positions and output them + int LegPos[NUM_LEGS][3] = { + {-WheelWidth/2,WheelLength/2,kInitialPositions[0][2]}, + {WheelWidth/2,WheelLength/2,kInitialPositions[1][2]}, + {WheelWidth/2,-WheelLength/2,kInitialPositions[2][2]}, + {-WheelWidth/2,-WheelLength/2,kInitialPositions[3][2]}}; + for(int i = 0; i < NUM_LEGS; i++){ + for(int j = 0; j < 3; j++){ + gait_gen_out[i][j] = LegPos[i][j]; + } + } + + + + + //Code that does the swerve steering-------------------------------------------------------- + //idea taken from http://www.simbotics.org/resources/mobility/omnidirectional-drive + float ModeA = Xmove - (Zrot*WheelLength/2); + float ModeB = Xmove + (Zrot*WheelLength/2); + float ModeC = Ymove - (Zrot*WheelWidth/2); + float ModeD = Ymove + (Zrot*WheelWidth/2); + + float LegWheelSpherical1[NUM_LEGS][3]{//Azimuth, Elevation, RotationSpeed + {atan2(ModeB,ModeC),0,sqrt(ModeB*ModeB+ModeC*ModeC)},//FR + {atan2(ModeA,ModeC),0,sqrt(ModeA*ModeA+ModeC*ModeC)},//BR + {atan2(ModeA,ModeD),0,sqrt(ModeA*ModeA+ModeD*ModeD)},//BL + {atan2(ModeB,ModeD),0,sqrt(ModeB*ModeB+ModeD*ModeD)}};//FL + + //duplicate values for scaling + float LegWheelSpherical2[4][3]; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 3; j++){ + LegWheelSpherical2[i][j] = LegWheelSpherical1[i][j]; + } + } + + + //Scale values so largest has a max of 1 + if (LegWheelSpherical1[0][2] > 1){ + for(int i = 0; i < 4; i++){ + LegWheelSpherical2[i][2] /= LegWheelSpherical1[0][2]; + } + } + if (LegWheelSpherical1[1][2] > 1){ + for(int i = 0; i < 4; i++){ + LegWheelSpherical2[i][2] /= LegWheelSpherical1[1][2]; + } + } + if (LegWheelSpherical1[2][2] > 1){ + for(int i = 0; i < 4; i++){ + LegWheelSpherical2[i][2] /= LegWheelSpherical1[2][2]; + } + } + if (LegWheelSpherical1[3][2] > 1){ + for(int i = 0; i < 4; i++){ + LegWheelSpherical2[i][2] /= LegWheelSpherical1[3][2]; + } + } + + //Set the spherical part + for(int i = 0; i < 3; i++){ + leg_global_spherical[i][0] = LegWheelSpherical1[1][0]; + leg_global_spherical[i][1] = LegWheelSpherical1[1][1]; + leg_global_spherical[i][2] += LegWheelSpherical1[1][2]*WheelVelocity; + } +} diff --git a/HexapodCodeV4/m_Display.ino b/HexapodCodeV4/m_Display.ino new file mode 100644 index 0000000..6a840a6 --- /dev/null +++ b/HexapodCodeV4/m_Display.ino @@ -0,0 +1,72 @@ +//display +/* provide control for Display + Requires changing Adafruit_SSD1306.cpp to change from wire.h to i2c_t3.h and commenting out compiler sections starting with "#ifndef __SAM3X8E__" + + display will show 21 characters per line at size 1 with 4 lines + */ + +void MainDisplay(){//Display run in loop +#if defined(OLED_ENABLE) + //Time=xxxxxxxxxxxxxxxx + //X00Y00Z00,R:X00Y00Z00 + //B:00,E:00,00,V:000, + //T:000 + display.clearDisplay(); + display.setTextSize(1); + display.setTextColor(WHITE); + display.setCursor(0,0); + display.print("Time = "); + display.println((float)time_1/1000); + + display.print("X"); + display.print(input_x+128,HEX); + display.print("Y"); + display.print(input_y+128,HEX); + display.print("Z"); + display.print(input_z+128,HEX); + display.print("R:X"); + display.print(input_rx+128,HEX); + display.print("Y"); + display.print(input_ry+128,HEX); + display.print("Z"); + display.println(input_rz+128,HEX); + + display.print("B:"); + display.print(input_buttons+128,HEX); + display.print(",E:"); + display.print(input_extend1+128,HEX); + display.print(","); + display.print(input_extend2+128,HEX); + display.print(",V:"); + display.println(current_voltage,DEC); + + display.print("T:"); + display.print(last_cycle_time,DEC); +#endif +} + +/* +#include + //#include //Conflicts with i2c_t3 + #include + #include + + #define OLED_RESET 27 + Adafruit_SSD1306 display(OLED_RESET); + + + display.clearDisplay(); + display.setTextSize(1); + display.setTextColor(WHITE); + display.setCursor(0,0); + display.println("Hello, world!"); + display.setTextColor(BLACK, WHITE); // 'inverted' text + display.println(3.141592); + display.setTextSize(2); + display.setTextColor(WHITE); + display.print("0x"); display.println(0xDEADBEEF, HEX); + display.display(); + delay(2000); + + */ + diff --git a/HexapodCodeV4/n_Gimbal.ino b/HexapodCodeV4/n_Gimbal.ino new file mode 100644 index 0000000..15ca844 --- /dev/null +++ b/HexapodCodeV4/n_Gimbal.ino @@ -0,0 +1,18 @@ +//gimbal +//Functions to control the Gimbal using PPM + + +void GimbalControl(float pos_x, float pos_y, float gimbal_command){//pos_x and _y go from -127 to 127, gimbal_command is 0,1,2 +#if defined(GIMBAL_ENABLE)//If Gimbal + pos_x = map(pos_x, -127, 127, 1000, 2000); + pos_x = map(pos_y, -127, 127, 1000, 2000); + gimbal_command = map(gimbal_command, 0, 2, 1000, 2000); + + GimbalOut.write(1, pos_x); + GimbalOut.write(1, pos_y); + GimbalOut.write(1, gimbal_command); +#endif // End Gimbal if +} + + +