SPI_EEP_ENC.cpp

00001 #include "mbed.h"
00002 #include "setting.h"
00003 #include "SPI_EEP_ENC.h"
00004 
00005 // EEPROM
00006 void spi_eeprom_ready(void){
00007     int temp1, temp2;
00008     do{
00009         eeprom_cs=0;
00010         eeprom.write(0x06);  //write enable
00011         eeprom_cs=1;
00012         
00013         eeprom_cs=0;
00014         temp1 = eeprom.write(0x05);     
00015         temp2 = eeprom.write(0x00);
00016         eeprom_cs=1;
00017         temp2=(temp2&(0x03))!= 0x02;
00018     } while(temp2); // before writing or reading 
00019 }
00020 // 
00021  void spi_eeprom_write(unsigned short add, unsigned int data){
00022         spi_eeprom_ready();
00023         add=add*4;
00024         eeprom_cs=0;
00025         eeprom.write(0x02);
00026         eeprom.write(0xff&(add>>8));
00027         eeprom.write(0xff&add);
00028         eeprom.write(0xff&data);
00029         eeprom.write(0xff&(data>>8));
00030         eeprom.write(0xff&(data>>16));
00031         eeprom.write(0xff&(data>>24));
00032         eeprom_cs=1;
00033 }
00034 // 
00035 unsigned int spi_eeprom_read(unsigned short add){
00036         add=add*4;
00037         eeprom_cs=0;
00038         eeprom.write(0x03);
00039         eeprom.write(0xff&(add>>8));
00040         eeprom.write(0xff&add);
00041 //        
00042         int a1 = eeprom.write(0x00);
00043         int a2 = eeprom.write(0x00);
00044         int a3 = eeprom.write(0x00);
00045         int a4 = eeprom.write(0x00);
00046         eeprom_cs=1;
00047         //unsigned int final = (a4<<24)+(a3<<16) + (a2<<8) + a1;
00048         unsigned int final = (int32_t) (a1 | a2 << 8 | a3 << 16 | a4 << 24);
00049         return final;
00050 } 
00051 
00052 void spi_eeprom_call_data(void)
00053 {
00054     BNO = spi_eeprom_read(RID_BNO);
00055     BNO = 11;
00056     OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
00057     SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE);
00058     SENSING_MODE = 1;
00059     CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE);
00060     CURRENT_CONTROL_MODE = 1;
00061     FLAG_VALVE_DEADZONE = spi_eeprom_read(RID_FLAG_VALVE_DEADZONE);
00062     CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
00063     CAN_FREQ = 500;
00064     DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
00065     DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
00066     DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
00067     SUPPLY_VOLTAGE = (float) (spi_eeprom_read(RID_VOLATGE_SUPPLY)) *0.1f;
00068     VALVE_VOLTAGE_LIMIT = (float) (spi_eeprom_read(RID_VOLTAGE_VALVE)) * 0.1f;
00069     P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
00070     I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
00071     D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
00072     P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
00073     I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
00074     D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
00075     P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
00076     I_GAIN_JOINT_TORQUE = spi_eeprom_read( RID_I_GAIN_JOINT_TORQUE);
00077     D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
00078     VALVE_DEADZONE_PLUS = (float) (spi_eeprom_read( RID_VALVE_DEADZONE_PLUS)) * 0.1f;
00079     VALVE_DEADZONE_MINUS = (float) (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS)) * 0.1f;
00080     VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
00081     COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
00082     VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
00083     VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
00084     BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
00085     CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
00086     CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
00087     PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
00088     PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
00089     PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
00090     alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
00091     PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
00092     PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
00093     ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
00094     ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
00095     STROKE = spi_eeprom_read(RID_STROKE);
00096     //VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
00097     //VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
00098     ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION)) * 0.1f;
00099 //    ENC_PULSE_PER_POSITION = (float) 1024.0f;
00100     TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
00101     //TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle
00102 //    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee
00103     PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
00104     PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
00105     PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
00106     PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
00107     FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
00108     HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
00109     HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
00110     TORQUE_VREF = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_VREF)) *0.001f;
00111     PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
00112     PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
00113     VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
00114     VALVE_GAIN_LPM_PER_V[2] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_2)) * 0.01f;
00115     VALVE_GAIN_LPM_PER_V[4] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_3)) * 0.01f;
00116     VALVE_GAIN_LPM_PER_V[6] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_4)) * 0.01f;
00117     VALVE_GAIN_LPM_PER_V[8] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_5)) * 0.01f;
00118     VALVE_GAIN_LPM_PER_V[1] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_1)) * 0.01f;
00119     VALVE_GAIN_LPM_PER_V[3] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_2)) * 0.01f;
00120     VALVE_GAIN_LPM_PER_V[5] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_3)) * 0.01f;
00121     VALVE_GAIN_LPM_PER_V[7] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_4)) * 0.01f;
00122     VALVE_GAIN_LPM_PER_V[9] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_5)) * 0.01f;
00123     for(int i=0; i<25; i++) {
00124         VALVE_POS_VS_PWM[i] = (float) (spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i));
00125     }
00126     for(int i=0; i<100; i++) {
00127         JOINT_VEL[i] = ( ((spi_eeprom_read( RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((spi_eeprom_read(RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
00128     }
00129     VALVE_MAX_POS = spi_eeprom_read(RID_VALVE_MAX_POS);
00130     VALVE_MIN_POS = spi_eeprom_read(RID_VALVE_MIN_POS);
00131     //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
00132     VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM);
00133     
00134     K_SPRING = spi_eeprom_read(RID_K_SPRING);
00135     D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
00136 
00137     
00138 }
00139  
00140  // ENCODER
00141 void spi_enc_set_clear(void){
00142     unsigned int temp;
00143     enc_cs = 0;
00144     temp = enc.write(0b00100000);
00145     enc_cs = 1;
00146 }
00147 
00148 void spi_enc_set_init(void){
00149     unsigned int temp, i, temp1, temp2;
00150 
00151     // write MDR0 -> 0b11 -> x4 quadrature count mode
00152     enc_cs = 0;
00153     temp = enc.write(0b10001000);     // WR + MDR0
00154     temp = enc.write(0b00000011);     // quadratue mode
00155     enc_cs = 1;
00156 
00157     // write MDR1 -> 0b10 -> 2-byte counter mode
00158     for(i=0;i<100;i++);
00159     enc_cs = 0;
00160     temp = enc.write(0b10010000);     // WR + MDR1
00161     //temp = enc.write(0b00000010);     // 2 byte mode
00162     temp = enc.write(0b00000000);     // 4 byte mode
00163     enc_cs = 1;
00164     
00165     // clear
00166     spi_enc_set_clear();
00167 }
00168 
00169 
00170 int spi_enc_read(void){   
00171     //for(t_i=0;t_i<100;t_i++);
00172     unsigned int t_dummy, t_b1, t_b2, t_b3, t_b4, t_i;
00173     enc_cs = 0;
00174     t_dummy = enc.write(0b01100000); // Read Commend 
00175     t_b1 = enc.write(0x00);         // Dummy data for clock
00176     t_b2 = enc.write(0x00);         // Dummy data for clock
00177     t_b3 = enc.write(0x00);         // Dummy data for clock
00178     t_b4 = enc.write(0x00);         // Dummy data for clock
00179     enc_cs = 1;
00180 
00181     return((t_b1<<24) + (t_b2<<16) + (t_b3<<8) + t_b4);
00182 }