ECE 4333
motion primitives
Dependencies: mbed
Fork of
Motion_Primitive_Test
by 
ECE 4333
Diff: MotionTest.cpp
- Revision:
- 6:96d4e0fa0827
- Parent:
- 5:ac5819613d0c
--- 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);
- }
-}