motion primitives
Dependencies: mbed
Fork of Motion_Primitive_Test by
Diff: MotionPrimitive.cpp
- Revision:
- 6:96d4e0fa0827
- Parent:
- 5:ac5819613d0c
diff -r ac5819613d0c -r 96d4e0fa0827 MotionPrimitive.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotionPrimitive.cpp Tue Apr 05 19:32:11 2016 +0000 @@ -0,0 +1,320 @@ +// 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; + + +// ********************************************************************* +// 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) + pc.printf("\n\r >> DE0 Initialized.\n\r");} + else{ + pc.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 < -limit){ + return(-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; + +} +// ********************************************************************* +// MAIN FUNCTION +// ********************************************************************* + +int main(){ + + // Initialization + + DE0_Init(0x8004); + MotorInit(); + ControlInterrupt.attach(&ControlThread, 0.00025); + IntegratorInit(); + float dL,dR,v,vR,vL; + + //float Motion[5][2] = { {30.5,47.9} , {10000,152.4} , {-61,287.3} , {10000,91.4} , {45.7,143.6} }; % Using 10k to represent inf + + float R[5]={30.5,100000,-61,100000,45.7}; + float d[5]={47.9*1.05,152.4,287.3,91.4,143.6}; + + pc.printf("\n\r ready to work"); + wait(5); + //int i=0; + for( int i = 0 ; i < 5 ; i++ ){ + + //pc.printf("\n\r off i go then"); + dL = d[i]*(1+L/(2*R[i]))/Rw*(851400/6804)*(100/pi)/1.9; + dR = d[i]*(1-L/(2*R[i]))/Rw*(851400/6804)*(100/pi)/1.9; + + v = 25; + setpoint_R = v*(1-L/(2*R[i]))/Rw; + setpoint_L = v*(1+L/(2*R[i]))/Rw; + + pc.printf("\n\r%2.2f",d[i]); + pc.printf("\n\r%2.2f",R[i]); + pc.printf("\n\r %2.2f",dL); + pc.printf("\n\r %2.2f",dR); + printf("\n\r%2.2f",setpoint_R); + printf("\n\r%2.2f",setpoint_L); + + while((PositionLeft<dL) || (PositionRight<dR)){ + pc.printf("\n\r%2.2f, %2.2f",setpoint_L,omega_L); + pc.printf("\n\r%2.2f, %2.2f",setpoint_R,omega_R); + pc.printf("\n\r"); + wait(0.1); + } + + PositionLeft = 0; + PositionRight = 0; + } + setpoint_R = 0; + setpoint_L = 0; +}