Shao Rui
/
AGV
1
main.cpp
- Committer:
- shaorui
- Date:
- 2021-01-25
- Revision:
- 0:571a1835428e
File content as of revision 0:571a1835428e:
#include "mbed.h" #include "struct.h" #include "PositionSensor.h" #include "PreferenceWriter.h" #include "brake.h" #include "user_config.h" PreferenceWriter prefs(6); float __float_reg[64]; // Floats stored in flash int __int_reg[256]; GPIOStruct gpio; ControllerStruct controller; Serial pc(PA_2, PA_3); PositionSensorAM5147 spi(16384, 0.0, 0); //14 bits encoder, 21 NPP int brake_state=0; int flag=0; Ticker flipper; void serial_interrupt(void){ while(pc.readable()){ char c = pc.getc(); switch (c){ case 'z': spi.SetMechOffset(0); spi.Sample(0.000025); wait_us(20); M_OFFSET = spi.GetMechPosition(); if (!prefs.ready()) prefs.open(); prefs.flush(); // Write new prefs to flash prefs.close(); prefs.load(); spi.SetMechOffset(M_OFFSET); printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); break; } } } void time1_check() { spi.Sample(0.000025); //pc.printf("%.3f\n\r",controller.theta_mech); // sample position sensor controller.theta_elec = spi.GetElecPosition(); controller.theta_mech = spi.GetMechPosition(); controller.dtheta_mech = spi.GetMechVelocity(); controller.dtheta_elec = spi.GetElecVelocity(); /* if(gpio.indicate->read()==1) { gpio.brake->write(1); } else{ gpio.brake->write(0); }*/ if(gpio.indicate->read()==1) { brake_state=1; } else{ brake_state=0; } controller.loop_count++; if(controller.loop_count>80000) { controller.loop_count=80000; } if(brake_state=1&&controller.loop_count>=80000) { flag=-flag; if(flag) { gpio.brake->write(1); } if(flag) { gpio.brake->write(0); } } if(brake_state=1&&controller.loop_count<80000) { gpio.brake->write(1); } else { gpio.brake->write(0); } } int main() { Init_Brake(&gpio); controller.loop_count=0; prefs.load(); // Read flash if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} spi.SetElecOffset(E_OFFSET); // Set position sensor offset spi.SetMechOffset(M_OFFSET); int lut[128] = {0}; memcpy(&lut, &ENCODER_LUT, sizeof(lut)); spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table pc.baud(115200); // set serial baud rate wait(.01); wait(.01); pc.attach(&serial_interrupt); // attach serial interrupt flipper.attach_us(&time1_check,25); while(1) { printf("%.3f\n\r",controller.theta_mech); printf("%.3d\n\r",spi.GetRawPosition()); wait_ms(1000); } }