Shao Rui
/
AGV
1
main.cpp@0:571a1835428e, 2021-01-25 (annotated)
- Committer:
- shaorui
- Date:
- Mon Jan 25 08:36:48 2021 +0000
- Revision:
- 0:571a1835428e
1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shaorui | 0:571a1835428e | 1 | #include "mbed.h" |
shaorui | 0:571a1835428e | 2 | #include "struct.h" |
shaorui | 0:571a1835428e | 3 | #include "PositionSensor.h" |
shaorui | 0:571a1835428e | 4 | #include "PreferenceWriter.h" |
shaorui | 0:571a1835428e | 5 | #include "brake.h" |
shaorui | 0:571a1835428e | 6 | #include "user_config.h" |
shaorui | 0:571a1835428e | 7 | PreferenceWriter prefs(6); |
shaorui | 0:571a1835428e | 8 | float __float_reg[64]; // Floats stored in flash |
shaorui | 0:571a1835428e | 9 | int __int_reg[256]; |
shaorui | 0:571a1835428e | 10 | GPIOStruct gpio; |
shaorui | 0:571a1835428e | 11 | ControllerStruct controller; |
shaorui | 0:571a1835428e | 12 | Serial pc(PA_2, PA_3); |
shaorui | 0:571a1835428e | 13 | PositionSensorAM5147 spi(16384, 0.0, 0); //14 bits encoder, 21 NPP |
shaorui | 0:571a1835428e | 14 | |
shaorui | 0:571a1835428e | 15 | int brake_state=0; |
shaorui | 0:571a1835428e | 16 | int flag=0; |
shaorui | 0:571a1835428e | 17 | Ticker flipper; |
shaorui | 0:571a1835428e | 18 | |
shaorui | 0:571a1835428e | 19 | void serial_interrupt(void){ |
shaorui | 0:571a1835428e | 20 | while(pc.readable()){ |
shaorui | 0:571a1835428e | 21 | char c = pc.getc(); |
shaorui | 0:571a1835428e | 22 | switch (c){ |
shaorui | 0:571a1835428e | 23 | case 'z': |
shaorui | 0:571a1835428e | 24 | spi.SetMechOffset(0); |
shaorui | 0:571a1835428e | 25 | spi.Sample(0.000025); |
shaorui | 0:571a1835428e | 26 | wait_us(20); |
shaorui | 0:571a1835428e | 27 | M_OFFSET = spi.GetMechPosition(); |
shaorui | 0:571a1835428e | 28 | if (!prefs.ready()) prefs.open(); |
shaorui | 0:571a1835428e | 29 | prefs.flush(); // Write new prefs to flash |
shaorui | 0:571a1835428e | 30 | prefs.close(); |
shaorui | 0:571a1835428e | 31 | prefs.load(); |
shaorui | 0:571a1835428e | 32 | spi.SetMechOffset(M_OFFSET); |
shaorui | 0:571a1835428e | 33 | printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); |
shaorui | 0:571a1835428e | 34 | |
shaorui | 0:571a1835428e | 35 | break; |
shaorui | 0:571a1835428e | 36 | } |
shaorui | 0:571a1835428e | 37 | |
shaorui | 0:571a1835428e | 38 | } |
shaorui | 0:571a1835428e | 39 | } |
shaorui | 0:571a1835428e | 40 | |
shaorui | 0:571a1835428e | 41 | void time1_check() |
shaorui | 0:571a1835428e | 42 | { |
shaorui | 0:571a1835428e | 43 | spi.Sample(0.000025); |
shaorui | 0:571a1835428e | 44 | //pc.printf("%.3f\n\r",controller.theta_mech); // sample position sensor |
shaorui | 0:571a1835428e | 45 | controller.theta_elec = spi.GetElecPosition(); |
shaorui | 0:571a1835428e | 46 | controller.theta_mech = spi.GetMechPosition(); |
shaorui | 0:571a1835428e | 47 | controller.dtheta_mech = spi.GetMechVelocity(); |
shaorui | 0:571a1835428e | 48 | controller.dtheta_elec = spi.GetElecVelocity(); |
shaorui | 0:571a1835428e | 49 | /* |
shaorui | 0:571a1835428e | 50 | if(gpio.indicate->read()==1) |
shaorui | 0:571a1835428e | 51 | { |
shaorui | 0:571a1835428e | 52 | gpio.brake->write(1); |
shaorui | 0:571a1835428e | 53 | } |
shaorui | 0:571a1835428e | 54 | else{ |
shaorui | 0:571a1835428e | 55 | gpio.brake->write(0); |
shaorui | 0:571a1835428e | 56 | }*/ |
shaorui | 0:571a1835428e | 57 | if(gpio.indicate->read()==1) |
shaorui | 0:571a1835428e | 58 | { |
shaorui | 0:571a1835428e | 59 | brake_state=1; |
shaorui | 0:571a1835428e | 60 | } |
shaorui | 0:571a1835428e | 61 | else{ |
shaorui | 0:571a1835428e | 62 | brake_state=0; |
shaorui | 0:571a1835428e | 63 | } |
shaorui | 0:571a1835428e | 64 | controller.loop_count++; |
shaorui | 0:571a1835428e | 65 | if(controller.loop_count>80000) |
shaorui | 0:571a1835428e | 66 | { |
shaorui | 0:571a1835428e | 67 | controller.loop_count=80000; |
shaorui | 0:571a1835428e | 68 | } |
shaorui | 0:571a1835428e | 69 | if(brake_state=1&&controller.loop_count>=80000) |
shaorui | 0:571a1835428e | 70 | { |
shaorui | 0:571a1835428e | 71 | flag=-flag; |
shaorui | 0:571a1835428e | 72 | if(flag) |
shaorui | 0:571a1835428e | 73 | { |
shaorui | 0:571a1835428e | 74 | gpio.brake->write(1); |
shaorui | 0:571a1835428e | 75 | } |
shaorui | 0:571a1835428e | 76 | if(flag) |
shaorui | 0:571a1835428e | 77 | { |
shaorui | 0:571a1835428e | 78 | gpio.brake->write(0); |
shaorui | 0:571a1835428e | 79 | } |
shaorui | 0:571a1835428e | 80 | } |
shaorui | 0:571a1835428e | 81 | if(brake_state=1&&controller.loop_count<80000) |
shaorui | 0:571a1835428e | 82 | { |
shaorui | 0:571a1835428e | 83 | gpio.brake->write(1); |
shaorui | 0:571a1835428e | 84 | } |
shaorui | 0:571a1835428e | 85 | else |
shaorui | 0:571a1835428e | 86 | { |
shaorui | 0:571a1835428e | 87 | gpio.brake->write(0); |
shaorui | 0:571a1835428e | 88 | } |
shaorui | 0:571a1835428e | 89 | |
shaorui | 0:571a1835428e | 90 | } |
shaorui | 0:571a1835428e | 91 | |
shaorui | 0:571a1835428e | 92 | int main() |
shaorui | 0:571a1835428e | 93 | { |
shaorui | 0:571a1835428e | 94 | Init_Brake(&gpio); |
shaorui | 0:571a1835428e | 95 | controller.loop_count=0; |
shaorui | 0:571a1835428e | 96 | prefs.load(); // Read flash |
shaorui | 0:571a1835428e | 97 | if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} |
shaorui | 0:571a1835428e | 98 | if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} |
shaorui | 0:571a1835428e | 99 | spi.SetElecOffset(E_OFFSET); // Set position sensor offset |
shaorui | 0:571a1835428e | 100 | spi.SetMechOffset(M_OFFSET); |
shaorui | 0:571a1835428e | 101 | int lut[128] = {0}; |
shaorui | 0:571a1835428e | 102 | memcpy(&lut, &ENCODER_LUT, sizeof(lut)); |
shaorui | 0:571a1835428e | 103 | spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table |
shaorui | 0:571a1835428e | 104 | pc.baud(115200); // set serial baud rate |
shaorui | 0:571a1835428e | 105 | wait(.01); |
shaorui | 0:571a1835428e | 106 | wait(.01); |
shaorui | 0:571a1835428e | 107 | pc.attach(&serial_interrupt); // attach serial interrupt |
shaorui | 0:571a1835428e | 108 | flipper.attach_us(&time1_check,25); |
shaorui | 0:571a1835428e | 109 | while(1) { |
shaorui | 0:571a1835428e | 110 | printf("%.3f\n\r",controller.theta_mech); |
shaorui | 0:571a1835428e | 111 | printf("%.3d\n\r",spi.GetRawPosition()); |
shaorui | 0:571a1835428e | 112 | |
shaorui | 0:571a1835428e | 113 | wait_ms(1000); |
shaorui | 0:571a1835428e | 114 | } |
shaorui | 0:571a1835428e | 115 | } |