WASD, R to stop, Q to go straight at same speed
Dependencies: mbed
main.cpp
- Committer:
- A_Sterner
- Date:
- 2016-04-05
- Revision:
- 1:b09a2f277b93
- Parent:
- 0:f8a9fb252057
File content as of revision 1:b09a2f277b93:
// EE4333 Robotics Lab 3 // Library Imports #include "mbed.h" #include "Serial.h" #include "stdio.h" // Function Declarations // DE0 void DE0_Init(int); // Done, AS - 1/4/16 // Motor void MotorInit(void); // Done, AS - 1/4/16 // Control Thread void ControlThread(void); void IntegratorInit(void); // Computational Functions float SaturateLimit(float x, float limit); // Done, AS - 1/4/16 signed int SignExtend(signed int x); // ******************************************************************** // Global Variable Declarations // ******************************************************************** float setpoint_L; // Desired speed (mm/sec) float setpoint_R; float error_L; // Velocity Error (mm/sec) float error_R; float amp_L; // PWM Amplifier Signal float amp_R; float int_L; // Integrator States float int_R; signed int dPositionLeft; // Position Data retrieved from DE0 signed int dPositionRight; int dTimeLeft; // Time Data retrieved from DE0 int dTimeRight; float omega_L; // Angular Velocity of Wheels float omega_R; signed int PositionLeft; // Incremented Position signed int PositionRight; float Rw = 2*2.54; // Radius of Wheels ( cm ) float L = 5.5*2.54; //Distance between wheel contacts ( cm ) float pi = 3.14159265359; float s = 0; float r = 0; // ********************************************************************* // Pin Declarations - Done, AS - 1/4/16 // ********************************************************************* 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); // Communication Channels Serial pc(USBTX, USBRX); // TX & RX for serial channel via USB cable Serial Bluetooth(p9,p10); // TX(p9) , RX(p10) for bluetooth serial channel // SPI : Communication wih DE0 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 - Done, AS - 1/4/16 // ************************************************************************************************** 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");} } // ************************************************************************************************** // Motor Initialization - Done, AS - 1/4/16 // ************************************************************************************************** void MotorInit(void){ int T = 100; DirL = 0; DirR = 1; PwmL.period_us(T); PwmL.write(0); // Motor Initially Off PwmR.period_us(T); PwmR.write(0); // Motor Initially Off } // ************************************************************************************************** // Control Thread - Done, AS - 1/4/16 // ************************************************************************************************** void ControlThread(void){ // Read Incremental Position from DE0 QEI int dummy = 0x0000; dPositionLeft = SignExtend(DE0.write(dummy)); dTimeLeft = DE0.write(dummy); dPositionRight = SignExtend(DE0.write(dummy)); dTimeRight = DE0.write(dummy); // Update Total Incremented Position PositionLeft = PositionLeft + dPositionLeft; PositionRight = PositionRight + dPositionRight; // Computer Angular Speed omega_L = (49*dPositionLeft)/dTimeLeft; omega_R = (49*dPositionRight)/dTimeRight; error_L = setpoint_L - omega_L; error_R = setpoint_R - omega_R; // PI Controller Gains float Kp_L = 4; float Ki_L = 0.010; float Kp_R = 4; float Ki_R = 0.010; // Saturate Inegrators if necessary, currently not checking for overflow in the addition if(abs(SaturateLimit((Kp_L*error_L+Ki_L*int_L)/14,1))<1){ int_L = int_L + error_L;} else{ int_L = int_L; } if(abs(SaturateLimit((Kp_R*error_R+Ki_R*int_R)/14,1))<1){ //Checks that we are not saturating the Left integrator before summing more error int_R = int_R + error_R;} else{ int_R = int_R; } amp_L = SaturateLimit((Kp_L*error_L + Ki_L*int_L),1); amp_R = SaturateLimit((Kp_R*error_R + Ki_R*int_R),1); //Determine direction bits based on sign of required output signal if(amp_L <=0) DirL = 0; else DirL = 1; if(amp_R <=0) DirR = 1; else DirR = 0; //Write final values to the PWM PwmL.write(abs(amp_L)); PwmR.write(abs(amp_R)); } /*// ************************************************* // SaturateAdd - Not Used // *************************************************** 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 - Done, AS - 1/4/16 // *************************************************** 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 < -1*limit){ return(-1*limit); } else{ return x;} } /// *************************************************** // SignExtend - Done, AS - 1/4/16 // *************************************************** signed int SignExtend(int x){ if(x&0x00008000){ x = x|0xFFFF0000; } return x; } /// *************************************************** // SignExtend - Done, AS - 1/4/16 // *************************************************** void IntegratorInit(void){ int_L = 0; int_R = 0; } // ********************************************************************** // Manual Control // ********************************************************************** void ManualControl(char c){ float ds = 1; float dr = 2; float rmax = 14; if (c=='w'||c=='W'){ r = r + dr;} else if(c=='s'||c=='S'){ r = r-dr;} else if(c=='a'||c=='A'){ s = s + ds;} else if(c=='d'||c=='D'){ s = s - ds;} else if(c=='q'||c=='Q'){ s = 0;} else if(c=='r'||c=='R'){ s = 0; r = 0;} else{ Bluetooth.printf("Invalid entry");} r = SaturateLimit(r,rmax); s = SaturateLimit(s,rmax/2); setpoint_R = r+s; setpoint_L = r-s; Bluetooth.printf("\n\rR :%2.2f",setpoint_R); Bluetooth.printf("\n\rL :%2.2f\n\r",setpoint_L); } // ********************************************************************* // MAIN FUNCTION // ********************************************************************* int main(){ // Initialization DE0_Init(0x8004); MotorInit(); ControlInterrupt.attach(&ControlThread, 0.00025); IntegratorInit(); char c; while(1){ c = Bluetooth.getc(); ManualControl(c); wait(0.1); } }