A code for the spindling of bots.
Dependencies: MX12 ServoRingBuffer mbed-src
Fork of SpindleBot by
main.cpp
- Committer:
- labmrd
- Date:
- 2015-08-13
- Revision:
- 14:7c5beaa9fb01
- Parent:
- 13:6a0a7a04fd91
File content as of revision 14:7c5beaa9fb01:
//#define USE_DYNAMIXELS #define USE_BLUETOOTH #define USE_SD_CARD // We have different modes for different things #define MODE_MANUAL 1 #define MODE_AUTOMATIC 2 #define MODE_IDLE 3 #define MODE_NULL 0 // We always want to know if we are closing or opening #define DIRECTION_CLOSING 1 #define DIRECTION_OPENING 2 #define DIRECTION_SLACK_WATER 3 #define DIRECTION_NULL 0 // General includes #include "mbed.h" #include "ServoRingBuffer.h" #include "ram_test.h" #include "Serial_Receive.h" #include <string> // Specific to Dynamixels #ifdef USE_DYNAMIXELS #include "MX12.h" #include "AD7730.h" #endif // Specific to SD Card #ifdef USE_SD_CARD #include "SDFileSystem.h" #endif // Everyone should know pi... #ifndef M_PI #define M_PI 3.14159265358979323846 /* pi */ #endif #ifndef M_PI_2 #define M_PI_2 1.57079632679489661923 /* pi/2 */ #endif // Create enum for the Jaw state (Closing, hold, opening) enum jaw_state{ STATE_CLOSING=0, STATE_CLOSE_HOLD=1, STATE_OPENING=2, STATE_OPEN_HOLD=3 }; // Define pins and interrupts Ticker potISR; //Define a recurring timer interrupt DigitalOut led1(LED1); //Led 1 for debugging purposes DigitalOut led2(LED2); //Led 2 for debugging purposes DigitalOut led3(LED3); //Led 3 for debugging purposes //DigitalOut led4(LED4); //Led 4 for debugging purposes DigitalOut triggerOut(p11); Serial pc(USBTX, USBRX); //Set up serial connection to pc #ifdef USE_BLUETOOTH Serial bt(p13,p14); //Set up serial connection to bluetooth adapter #endif AnalogIn AinLeftForce(p16); //Set up potentiometer on pin 20 AnalogIn AinRightForce(p15); //Set up potentiometer on pin 20 #ifdef USE_SD_CARD // Attach SD card SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board FILE *fp = NULL; #define SAMPLES_PER_FILE 10000 #endif // Dummy variable for debugging unsigned int global_count=0; float max_percent_full=0; // Define variables for the program float servoAngle; //This is the desired servo angle based on the scaled potentiometer value float potData; //This is the value of the potentiometer from Ain.read() bool collect_data = false; //This is bool keyStrokeFlag = false; //This is a flag to see if a keystroke has been pressed char keyStrokeVal; //This is a character storing the value of the keystroke char g_tissue_type_name[32]; float g_frequency; int g_max_force; int g_num_cycles; float g_current_trajectory_time; float g_theta; float g_theta_last=0; unsigned char g_current_mode=MODE_NULL; jaw_state g_current_direction=STATE_OPEN_HOLD; unsigned char g_current_cycle=0; // Warning, this buffer is large! ServoRingBuffer Buffer; spindleData tempSpindleData; //For sending to the buffer Timer ISRDurationTimer; Timer AuxSerialTimer; int worst_latency=0; int current_latency; #ifdef USE_DYNAMIXELS //Dynamixels can only handle 500Hz for now. Working on it... float samplingPeriod = 0.005; //This is the sampling period for the timer interrupt #define LEFT_JAW_DYNAMIXEL_ID 3 #define RIGHT_JAW_DYNAMIXEL_ID 4 #define CLOSED_SERVO_ANGLE_LEFT 1121 //This is the closed in encoder counts #define OPEN_SERVO_ANGLE_LEFT 2783 //This is the open in encoder counts #define CLOSED_SERVO_ANGLE_RIGHT 3259 //This is the closed in encoder counts #define OPEN_SERVO_ANGLE_RIGHT 1486 //This is the open in encoder counts // Dynamixel Object MX12 mx12_left_jaw (p28, p27, p30, p29, LEFT_JAW_DYNAMIXEL_ID, 1000000); MX12 mx12_right_jaw (p28, p27, p30, p29, RIGHT_JAW_DYNAMIXEL_ID, 1000000); AD7730 adc(p9, p26, p11, p12, p25); /// Set these to inputs so that they don't interfere with the serial communication DigitalIn nullOut1(p21); DigitalIn nullOut2(p22); DigitalIn nullOut3(p23); DigitalIn nullOut4(p24); #else float samplingPeriod = 0.001; //This is the sampling period for the timer interrupt #define SERVO_DEGREE_0 900 //This is the pulse width value for HiTEC-422 in microseconds to turn 0 degrees #define SERVO_DEGREE_180 2100 //This is the pulse width value for HiTEC-422 in microseconds to turn 180 degrees #define MIN_SERVO_ANGLE 0.0 //This is the minimum servo angle in degrees #define MAX_SERVO_ANGLE 180.0 //This is the maximum servo angle in degrees #define MIN_SERVO_ANGLE_Da_VINCI 20.0 //This is the minimum servo angle in degrees #define MAX_SERVO_ANGLE_Da_VINCI 100.0 //This is the maximum servo angle in degrees const float servoConversion = ((SERVO_DEGREE_180-SERVO_DEGREE_0)/(MAX_SERVO_ANGLE - MIN_SERVO_ANGLE))/1000000.0; //This is the interpolation between min and max servo values const float servoOffset = SERVO_DEGREE_0/1000000.0; //This is the pulsewidth value (in seconds) that corresponds to 0 degrees (i.e.-the offset) PwmOut myServoLeft(p21); //Set up servo on pin 21 PwmOut myServoRight(p22); //Set up servo on pin 22 AnalogIn AinLeftPosition(p20); //Set up potentiometer on pin 20 AnalogIn AinRightPosition(p19); //Set up potentiometer on pin 20 // Function moveServoTo: Convert a degree value to pulsewidth for Servo void moveServoTo(float angle) { // Make sure none of the user input falls outside of min and max angle limits if( angle < MIN_SERVO_ANGLE){angle = MIN_SERVO_ANGLE;} else if(angle > MAX_SERVO_ANGLE){angle = MAX_SERVO_ANGLE;} myServoLeft.pulsewidth(servoOffset + servoConversion*(180-angle)); myServoRight.pulsewidth(servoOffset + servoConversion*(angle)); } #endif // Function trapezoidalTrajectory: Function that takes in a time (float in seconds) and outputs a float (0 to 1) that corresponds to a trapezoidal trajectory float trapezoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) { // Define variables specific to this function float y_trapezoid = 0.0; float timeMod; float modifiedFrequency = g_frequency/2.0; float period = 1/modifiedFrequency; cycle_num=t*modifiedFrequency; // Take the time and mod it with the period to be able to break up each cycle into 4 piecewise sections timeMod = fmodf(t,period); // if (timeMod < period/4.0) { y_trapezoid = (-4.0/period)*(timeMod)+1.0; state = STATE_CLOSING; } else if (timeMod >= period/4.0 && timeMod < period/2.0) { y_trapezoid = 0.0; state = STATE_CLOSE_HOLD; } else if (timeMod >= period/2.0 && timeMod < 3*period/4.0) { y_trapezoid = (4.0/period)*(timeMod)-2; state = STATE_OPENING; } else if (timeMod >= 3*period/4.0) { y_trapezoid = 1.0; state = STATE_OPEN_HOLD; } return y_trapezoid; } void sinusoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) { //Fill me with SCIENCE!!! } // Function timerISRFunction: Timer ISR function to collect data and write to ring buffer void timerISRFunction() { if(collect_data){ //led 1 is used as a 'thinking' light, brighter=worse led1 = 1; led2 = 0; triggerOut = 1; ISRDurationTimer.reset(); ISRDurationTimer.start(); // Warning, this calculation is in the ISR and as such is probably slower than we would prefer. // @todo The math could certainly be optimized with some precalculated constants. Lookup tables are faster than sin() float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle); g_current_trajectory_time+=samplingPeriod; //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2; //g_current_direction=(cos(angle)<0); //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI); #ifdef USE_DYNAMIXELS //float percent=(sin(angle)+1)/2.0; if(adc.isReady()){ adc.interruptRead(); } short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ; short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT; mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,left_servo, 0, RIGHT_JAW_DYNAMIXEL_ID,right_servo, 0); // tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = adc.read(); // tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = mx12_left_jaw.GetRawPosition(); // tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16(); // tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = mx12_right_jaw.GetRawPosition(); // tempSpindleData.direction=g_current_direction; // tempSpindleData.cycle=g_current_cycle; // Buffer.write(tempSpindleData); #else g_theta=(1.0-percent)*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI; tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = AinLeftForce.read_u16(); tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = AinLeftPosition.read_u16(); tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16(); tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = AinRightPosition.read_u16(); tempSpindleData.direction=g_current_direction; tempSpindleData.cycle=g_current_cycle; Buffer.write(tempSpindleData); moveServoTo(g_theta); // in degrees, son #endif //done thinking led1 = 0; led2 = 1; triggerOut = 0; ISRDurationTimer.stop(); current_latency=ISRDurationTimer.read_us(); if(current_latency>worst_latency){ worst_latency=current_latency; } } } int main() { // Crazy fast baud rate! pc.baud(921600); #ifdef USE_BLUETOOTH bt.baud(9600); #endif // Attach ISR routines potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod // Some debug info: //DisplayRAMBanks(); //printf ("System clock = %d\r\n", SystemCoreClock); pc.printf("\n\n\n"); pc.printf("----------------------------------\n"); pc.printf("| |\n"); pc.printf("| Welcome to our mbed! |\n"); pc.printf("| |\n"); pc.printf("| John and Trevor, Proprietors |\n"); pc.printf("| |\n"); pc.printf("----------------------------------\n"); pc.printf(" ||\n"); pc.printf(" ||\n"); pc.printf(" || _\n"); pc.printf(" || _( )_\n"); pc.printf(" || (_(#)_)\n"); pc.printf(" || (_)\\\n"); pc.printf(" || | __\n"); pc.printf(" \\ / | || |/_/\n"); pc.printf(" / | | / / / | || | \\ \\ | / \n"); pc.printf("/ / \\ \\ / / / || / | / / \\ \\ \n"); pc.printf("#################################\n"); pc.printf("#################################\n"); pc.printf("\n\n"); #ifdef USE_DYNAMIXELS mx12_left_jaw.Init(); //mx12_left_jaw.SetBaud(3000000); //mx12_left_jaw.SetBaud(1000000); //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition()); mx12_right_jaw.Set_Return_Delay_Time(0.050); printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time()); mx12_left_jaw.Set_Return_Delay_Time(0.050); //mx12_left_jaw.Set_Torque_Limit(99.9); //mx12_right_jaw.Set_Torque_Limit(99.9); mx12_left_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF); mx12_right_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF); mx12_left_jaw.Set_P_Gain(4); mx12_right_jaw.Set_P_Gain(4); mx12_left_jaw.Set_I_Gain(8); mx12_right_jaw.Set_I_Gain(8); mx12_left_jaw.Set_Alarm_Shutdown(0x04); mx12_right_jaw.Set_Alarm_Shutdown(0x04); mx12_left_jaw.Dump_OD_to_Serial(pc); mx12_right_jaw.Dump_OD_to_Serial(pc); adc.setFilter(256 , false, 1); adc.start(); #else // Configure Servo for HiTec 422 myServoLeft.period_ms(20); myServoRight.period_ms(20); #endif printf("Setup Complete.\n"); AuxSerialTimer.start(); while(1) { // spin in a main loop. serialISR will interrupt it to call serialPot ///This checks for any new serial bytes, and returns true if ///we have an entire packet ready. The new packet will live ///in newData. if( #ifdef USE_BLUETOOTH receivePacket(bt) #else receivePacket(pc) #endif ) { // < Tissue Type (string), Frequency Value (Hz) (int), Force Max (N) (int), # Cycles (in) > //<date/tissue/time,2,3,4> //g_tissue_type_name=tissue_type_name; std::string file_name_in=inString.substr(0, inString.find(",")); g_frequency=newData[1]/10.0; // Since all we send are ints g_max_force=newData[2]; g_num_cycles=newData[3]; g_current_trajectory_time=0; g_current_cycle=0; g_current_mode=MODE_AUTOMATIC; #ifdef USE_SD_CARD int first_slash=file_name_in.find("/"); std::string new_dir="/sd/"+file_name_in.substr(0, first_slash); std::string new_subdir="/sd/"+file_name_in.substr(0, file_name_in.find("/",first_slash+1)); mkdir(new_dir.c_str(), 0777); mkdir(new_subdir.c_str(), 0777); std::string file_name="/sd/"+file_name_in+".csv"; //pc.printf("subdir=\"%s\"\n",file_name.c_str()); fp = fopen(file_name.c_str(), "w"); //FILE *fp = fopen("/sd/data/sdtest.txt", "w"); if(fp == NULL) { error("Could not open file for write\n"); } fprintf(fp, "%%Starting New Trajectory\n"); fprintf(fp, "%%File Name=\"%s\"\n",file_name.c_str()); fprintf(fp, "%%Current Mode=AUTOMATIC\n"); fprintf(fp, "%%Trajectory Type=TRAPEZOIDAL\n"); fprintf(fp, "%%Frequency=%f Hz\n",g_frequency); fprintf(fp, "%%Max Force=%f ??\n",g_max_force); fprintf(fp, "%%Num Cycles=%d\n",g_num_cycles); fprintf(fp, "%%Re. Direction: ,Closing=%d,Opening=%d,Undef=%d\n", DIRECTION_CLOSING , DIRECTION_OPENING , DIRECTION_SLACK_WATER ); fprintf(fp, "%%PositionLeft,ForceLeft,PositionRight,ForceRight,Time(ms),Direction,CycleNum\n"); #endif // We are go-times! collect_data=true; } if( collect_data && g_current_cycle >= g_num_cycles) { // STOOOOOOOOOP collect_data=false; #ifdef USE_SD_CARD // Close the file fclose(fp); fp = NULL; #endif } // This section of code should run whenever there is free time to print to the screen #ifdef USE_SD_CARD if(fp != NULL) { // Only write to SD if there is a valid file handle led3 = 1; Buffer.dumpBufferToSD(fp); led3 = 0; } #else Buffer.dumpBufferToSerial(); #endif if(AuxSerialTimer.read_ms()>100 && collect_data){ //Send some extra data for GUI purposes printf("<%d,%d,%d,%d,%d,%d,%d> ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos, tempSpindleData.myServoData[LEFT_SERVO_INDEX].force, tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos, tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force, tempSpindleData.time, tempSpindleData.direction, tempSpindleData.cycle); printf(" %dus\n", worst_latency); worst_latency=0; AuxSerialTimer.reset(); } } }