Shao Rui / Mbed 2 deprecated AGV

1

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }