motion primitives
Dependencies: mbed
Fork of Motion_Primitive_Test by
Diff: MotionTest.cpp
- Revision:
- 6:96d4e0fa0827
- Parent:
- 5:ac5819613d0c
diff -r ac5819613d0c -r 96d4e0fa0827 MotionTest.cpp --- a/MotionTest.cpp Sat Mar 19 19:50:53 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,394 +0,0 @@ -// 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); - char GUI(); - void Settings(char 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; - float Circ = 0; - float tTot = 0; - float AngularSpeedLeft; - float AngularSpeedRight; - //signed int AngularSpeedLeft; - //signed int AngularSpeedRight; - - //Motion Primitive Variables - signed int Arc; - float Radius = 0; - float VelDes = 0; - float tcalc = 0; - - float O_setpoint; //Inner Wheel Speed - float I_setpoint; //Outer Wheel Speed - float L = 5.5*2.54/100; //Distance between wheel contacts ; //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); - -} - - -/// *************************************************** -// 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)); //Extends the 16-bit signed ints into 32-bit containers - dTimeLeft = DE0.write(dummy); - dPositionRight = SignExtend(DE0.write(dummy)); - dTimeRight = DE0.write(dummy); - - // Computer Angular Speed and Angular Speed Error - - AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft; //123 is the constant derived from the gear ratios and properties of the motor - 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; - - // Saturate Inegrators if necessary - if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){ //Checks that we are not saturating the Left integrator before summing more error - 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){ //Checks that we are not saturating the Left integrator before summing more error - R_integrator = R_integrator +R_e;} - else{ - R_integrator = R_integrator; - } - - L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1); //Checks that we are not saturating the Left integrator before summing more error - R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1); //Checks that we are not saturating the Left integrator before summing more error - - //Determine direction bits based on sign of required output signal - if(L_u <=0) - DirL = 0; - else - DirL = 1; - - if(R_u <=0) - DirR = 1; - else - DirR = 0; - - //Write final values to the PWM - PwmL.write(abs(L_u)); - PwmR.write(abs(R_u)); -} - -/*// *************************************************** NOT USED -// 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){ //Ensures we don't ask for more than the PWM can give. - - 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; -} - -// *************************************************** -// Startup GUI -// *************************************************** - -char GUI(){ - char x; - Bluetooth.printf("\n\r (C)ircle or (L)ine? >>"); //Determine Trajectory Type - Bluetooth.scanf("%c", &x); - return x; //Return trajectory type - } - -// *************************************************** -// Setpoint Collection -// *************************************************** - - -void Settings(char x){ - char dir; //Direction Variable - float dist; //Distance Variable - if(x == 'c' || x == 'C') //If the user chooses a circle - { - Bluetooth.printf("\n\rCircle Radius? (m)"); //Determine circle radius - Bluetooth.scanf("%f", &Radius); - Bluetooth.printf("\n\rDesired Speed? (m)"); //Speed - Bluetooth.scanf("%f", &VelDes); - Bluetooth.printf("\n\r Desired Direction? (L) or (R)"); //Direction of Travel - Bluetooth.scanf("%c",&dir); - Bluetooth.printf("\n\r Desired Distance? (m)"); //Determine maximum distance - Bluetooth.scanf("%c",&dist); - - float Circ = 2*pi*Radius; - float tTot = Circ/VelDes; - - if(dir == 'l' || dir == 'L') //If counterclockwise direction, the right wheel is faster - { - I_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot); //Set right wheel speed - if(I_setpoint>35) - { - I_setpoint = 35; - Bluetooth.printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094); - O_setpoint = I_setpoint*(Radius-L/2)/(Radius+L/2); - return - } - } - else //Clockwise, the left wheel is faster - { - O_setpoint = (Radius+L/2)*2*pi/((2*2.54/100)*tTot); //Set Left wheel speed - if(O_setpoint>35) - { - O_setpoint = 35; - Bluetooth.printf("Setpoint limit exceeded. Capping outer wheel speed at %2.2f m/s", O_setpoint*0.05094); - - } - I_setpoint = (Radius-L/2)*2*pi/((2*2.54/100)*tTot); - } - } - else //If the user chooses a line instead - { - Bluetooth.printf("\n\r Desired Speed? (m) >>"); //We only need maximum speed and maximum distance - Bluetooth.scanf("%f", &VelDes); - Bluetooth.printf("\n\r Desired Distance? (m)"); //Determine maximum distance - Bluetooth.scanf("%c",&dist); - O_setpoint = VelDes/0.05094; //Saturate if necessary - - if(O_setpoint>35) - { - O_setpoint = 35; - Bluetooth.printf("\n\r Speed maximum exceeded. Capping outer wheel speed at %f m/s", O_setpoint*0.05094); - } - I_setpoint = O_setpoint; //For a straight line both wheel speeds must be equal - } - - - return; -} - - -// ********************************************************************* -// MAIN FUNCTION -// ********************************************************************* - -int main(){ - - // Initialization - - DE0_Init(0x8004); - L_MotorInit(); - R_MotorInit(); - L_integrator = 0; - R_integrator = 0; - ControlInterrupt.attach(&ControlThread, 0.0005); - float WAvg,VAvg; //Reported Speeds - - //Motion Primitives Initialization - Arc = 0; // Arc Length (start at 0) - Settings(GUI()); // Ask user for input and pass to Settings - - - // Display Global Variables to Console - while(1){ - WAvg = (AngularSpeedLeft + AngularSpeedRight); //Angular Average - VAvg = (AngularSpeedLeft + AngularSpeedRight)*0.02483*2; //Velocity Average with unit conversion - Bluetooth.printf("\n\r||SetLeft: %2.3f ||WLeft: %2.3f rad/s ||SetRight: %2.3f ||WRight: %2.3f rad/s||WAverage: %2.3f||VAverage: %2.3f",O_setpoint,AngularSpeedLeft,I_setpoint,AngularSpeedRight,WAvg,VAvg); - wait(0.75); - } -}