WASD, R to stop, Q to go straight at same speed
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:f8a9fb252057
- Child:
- 1:b09a2f277b93
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Apr 05 19:22:11 2016 +0000 @@ -0,0 +1,324 @@ +// 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 = 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); + } +} \ No newline at end of file