motion primitives
Dependencies: mbed
Fork of Motion_Primitive_Test by
MotionTest.cpp
- Committer:
- JordanWisdom
- Date:
- 2016-03-11
- Revision:
- 0:4c63f2192998
- Child:
- 1:d523415c7b53
File content as of revision 0:4c63f2192998:
// EE4333 Robotics Lab 3 // Library Imports //#include "InterruptIn.h" //#include "rtos.h" #include "mbed.h" #include "Serial.h" #include "stdio.h" //Function Declarations void DE0_Init(int); void L_MotorInit(void); void R_MotorInit(void); signed int InputLeft(void); signed int InputRight(void); void ControlThread(void); int SaturateAdd(int x, int y); float SaturateLimit(float x, float limit); signed int SignExtend(signed int x); // ******************************************************************** // GLOBAL VARIABLE DECLARATIONS // ******************************************************************** signed int R_setpoint; // Desired Angular Speed ( rad/sec ) signed int L_setpoint; signed int R_setpoint_m; // Desired Angular Speed ( m/sec ) signed int L_setpoint_m; float R_e; // Velocity Error float R_u; // Control Signal float L_e; float L_u; int L_integrator; // Left Integrator State int R_integrator; signed int dPositionLeft; // DE0 Register 0 signed int dPositionRight; int dTimeLeft; // DE0 Register 1 int dTimeRight; //Motion Primitive Variables signed int Arc; float Radius; float VelDes; float tcalc; int tcum; float O_setpoint; //Inner Wheel Speed float I_setpoint; //Outer Wheel Speed float L; //Distance between wheels (in meters) float pi = 3.141259; // ********************************************************************* // PROCESSES AND THREADS // ********************************************************************* // ********************************************************************* // PIN DECLARATIONS // ********************************************************************* // Digital I/O Pins DigitalOut led1(LED1); // Thread Indicators DigitalOut led2(LED2); // DigitalOut led3(LED3); // DigitalOut led4(LED4); // DigitalOut DirL(p29); // Direction of Left Motor DigitalOut DirR(p30); // Direction of Right Motor // SPI Related Digital I/O Pins DigitalOut SpiReset(p11); DigitalOut IoReset(p12); //PWM PwmOut PwmL(p22); PwmOut PwmR(p21); //Serial Serial pc(USBTX, USBRX); // tx and rx for PC serial channel via USB cable Serial Bluetooth(p9,p10); // Pins tx(p9) , rx(p10) for bluetooth serial channel //SPI SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK //Interrupts Ticker ControlInterrupt; // Internal Interrupt to trigger Control Thread // *************************************************** // DE0 Init // *************************************************** void DE0_Init(int SpiControlWord){ int mode = 1; int bits = 16; DE0.format(bits,mode); // Verify Peripheral ID // Generates single square pulse to reset DE0 IO IoReset = 0; IoReset = 1; IoReset = 0; // Generates single square pulse to reset DE0 SPI SpiReset = 0; SpiReset = 1; SpiReset = 0; // Writes to DE0 Control Register int ID = DE0.write(SpiControlWord); // SPI Control Word specifies SPI settings if(ID == 23){ // DE0 ID 23 (0x0017) Bluetooth.printf("\n\r >> DE0 Initialized.\n\r");} else{ Bluetooth.printf("\n\r >> Failed to initialize DE0 board.\n\r");} } // *************************************************** // Left Motor Initialization // *************************************************** // Pwm Pin Left Motor : p21 // Direction Pin Left Motor : p29 void L_MotorInit(void){ DirL = 1; // Defaults to 1 // Direction bit logic output // 0 : Backwards ( Reverse ) // 1 : Forwards ( Advance ) PwmL.period_us(100); PwmL.write(0); } // *************************************************** // Right Motor Initialization // *************************************************** // Pwm Pin Right Motor : p22 // Direction Pin Right Motor : p30 void R_MotorInit(void){ DirR = 0; // Defaults to 0. // Direction bit logic output // 0 : Forwards ( Advance ) // 1 : Backwards ( Reverse ) PwmR.period_us(100); PwmR.write(0); } /// *************************************************** // User Input Left // *************************************************** signed int InputLeft(void){ signed int input; Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (rads/sec) >> "); Bluetooth.scanf("%i",&input); Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input); return input; } signed int InputLeft_m(void){ signed int input; Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (m/sec) >> "); Bluetooth.scanf("%i",&input); Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input); return input; } /// *************************************************** // User Input Right // *************************************************** signed int InputRight(void){ signed int input; Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (rads/sec) >> "); Bluetooth.scanf("%i",&input); Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input); return input; } signed int InputRight_m(void){ signed int input; Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (m/sec) >> "); Bluetooth.scanf("%i",&input); Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input); return input; } /// *************************************************** // Control Thread // *************************************************** void ControlThread(void){ // Read Incremental Position from DE0 QEI int dummy = 0x0000; // Pushes dummy information which DE0 ignores, store return from QEI register dPositionLeft = SignExtend(DE0.write(dummy)); dTimeLeft = DE0.write(dummy); dPositionRight = SignExtend(DE0.write(dummy)); dTimeRight = DE0.write(dummy); // Computer Angular Speed and Angular Speed Error signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft; signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight; //CHANGED FOR OUTER AND INNER WHEELS L_e = O_setpoint - AngularSpeedLeft; R_e = I_setpoint - AngularSpeedRight; float Kp_L = 2.5; float Ki_L = 0.010; float Kp_R = 2.5; float Ki_R = 0.010; // **** MODIFICATIONS FOR MOTION PRIMITIVE ARE HERE **** // In order to determine theta relative to any given position, we can just use Theta = Arc Length/Radius // Radius R is determined by the user // Arc Length is determined from an integration of average velocity dP/dt, which is tracked by the DE0 board //Arc Length (meters) variable - We have speed and time outputs already so we can use those values to calculate average arc of V Arc = (AngularSpeedLeft*dTimeLeft+AngularSpeedRight*dTimeRight)*0.05093/2; //constant of 0.05093 derived in another function using wheel radii // Saturate Inegrators if necessary if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){ L_integrator = L_integrator +L_e;} else{ L_integrator = L_integrator; } if(abs(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1))<1){ R_integrator = R_integrator +R_e;} else{ R_integrator = R_integrator; } L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1); R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1); if(L_u <=0) DirL = 0; else DirL = 1; if(R_u <=0) DirR = 1; else DirR = 0; PwmL.write(abs(L_u)); PwmR.write(abs(R_u)); } /// *************************************************** // SaturateAdd // *************************************************** signed int SaturateAdd(signed int x, signed int y){ signed int z = x + y; if( (x>0) && (y>0)&& (z<=0) ){ z = 0x7FFFFFFF;} else if( (x<0)&&(y<0)&&(z>=0) ){ z = 0x80000000;} return z; } /// *************************************************** // SaturateLimit // *************************************************** float SaturateLimit(float x, float limit){ if (x > limit){ return limit; } else if(x < -limit){ return(-limit); } else{ return x;} } /// *************************************************** // Sign Extend // *************************************************** signed int SignExtend(int x){ if(x&0x00008000){ x = x|0xFFFF0000; } return x; } // ============================================================================================================== // ============================================================================================================== // ********************************************************************* // MAIN FUNCTION // ********************************************************************* int main(){ // Initialization DE0_Init(0x8004); L_MotorInit(); R_MotorInit(); L_integrator = 0; R_integrator = 0; ControlInterrupt.attach(&ControlThread, 0.0005); //Motion Primitives Initialization Arc = 0; // Arc Length (start at 0) L = 0.14605; //Distance between wheel contacts //IF RADIUS IS TOO LOW OR VELOCITY TOO HIGH THINGS GO CRAZY Radius = 0.2; // Minimum L/2; VelDes = 1.7; tcum = 0; float circum = 2*pi*Radius; //Arc circumference tcalc = circum/VelDes; // The easiest way to get the right speeds is to determine the theoretical completion time // Inner and Outer Wheel Setpioints O_setpoint = ((Radius+(L/2))*2*pi)/(2*2.54*tcalc/100); //0.02*2.54*tcalc is for rads/s I_setpoint = ((Radius-(L/2))*2*pi)/(2*2.54*tcalc/100); pc.printf("Out: %2.2f In: %2.2f \n\r", O_setpoint,I_setpoint); // Specify Setpoint ( rads/sec ) //L_setpoint = InputLeft(); //R_setpoint = InputRight(); // Specify Setpoint ( m/sec ) /* L_setpoint = InputLeft_m(); R_setpoint = InputRight_m();*/ // Display Global Variables to Console while(1){ float L_error_t = L_e; float L_error_m = L_e*0.05093; //Multiply by 0.05093 for m/s float L_u_t = L_u; float R_error_t = R_e; //Multiply by 0.05093 for m/s float R_u_t = R_u; float R_error_m = R_e*0.05093; //Multiply by 0.05093 for m/s float L_setpoint_m = L_setpoint*0.05093; //Setpoints in float values for display float R_setpoint_m = R_setpoint*0.05093; //Setpoints in float values for display pc.printf("Radius: %2.2f m Velocity: %2.2f rad/s\n\r", Radius,VelDes); if (O_setpoint > 35) { pc.printf("L_Set: MAX R_Set: MAX \n\r"); O_setpoint = 35; I_setpoint = O_setpoint*(Radius-L/2)/(Radius+L/2); } else { pc.printf("L_Set: %2.2f R_Set: %2.2f \n\r",O_setpoint,I_setpoint); } wait(0.75); } }