Working Motion Primitive Test Code
Dependencies: mbed
MotionTest.cpp
- Committer:
- JordanWisdom
- Date:
- 2016-03-19
- Revision:
- 5:ac5819613d0c
- Parent:
- 4:1bcb35cef400
File content as of revision 5:ac5819613d0c:
// 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); } }