Nicolas Borla / Mbed OS BBR_1Ebene

BBR 1 Ebene

main.cpp@0:fbdae7e6d805, 2018-05-14 (annotated)

Committer:
borlanic
Date:
Mon May 14 11:29:06 2018 +0000
Revision:
0:fbdae7e6d805
BBR

Who changed what in which revision?

UserRevisionLine numberNew contents of line
borlanic 0:fbdae7e6d805 1 #include "mbed.h"
borlanic 0:fbdae7e6d805 2 #include "EncoderCounter.h"
borlanic 0:fbdae7e6d805 3 #include "IMU.h"
borlanic 0:fbdae7e6d805 4 #include "Controller.h"
borlanic 0:fbdae7e6d805 5 #include "SerialCom.h"
borlanic 0:fbdae7e6d805 6
borlanic 0:fbdae7e6d805 7 using namespace std;
borlanic 0:fbdae7e6d805 8
borlanic 0:fbdae7e6d805 9 class Main
borlanic 0:fbdae7e6d805 10 {
borlanic 0:fbdae7e6d805 11
borlanic 0:fbdae7e6d805 12 public:
borlanic 0:fbdae7e6d805 13
borlanic 0:fbdae7e6d805 14 Main();
borlanic 0:fbdae7e6d805 15 virtual ~Main();
borlanic 0:fbdae7e6d805 16
borlanic 0:fbdae7e6d805 17 private:
borlanic 0:fbdae7e6d805 18
borlanic 0:fbdae7e6d805 19 static const uint32_t STACK_SIZE = 8192; // stack size of thread, given in [bytes]
borlanic 0:fbdae7e6d805 20
borlanic 0:fbdae7e6d805 21 Thread thread;
borlanic 0:fbdae7e6d805 22 void run();
borlanic 0:fbdae7e6d805 23 };
borlanic 0:fbdae7e6d805 24
borlanic 0:fbdae7e6d805 25 Main::Main() : thread(osPriorityNormal, STACK_SIZE)
borlanic 0:fbdae7e6d805 26 {
borlanic 0:fbdae7e6d805 27
borlanic 0:fbdae7e6d805 28 thread.start(callback(this, &Main::run));
borlanic 0:fbdae7e6d805 29 }
borlanic 0:fbdae7e6d805 30
borlanic 0:fbdae7e6d805 31 Main::~Main() {}
borlanic 0:fbdae7e6d805 32
borlanic 0:fbdae7e6d805 33 void Main::run()
borlanic 0:fbdae7e6d805 34 {
borlanic 0:fbdae7e6d805 35
borlanic 0:fbdae7e6d805 36 DigitalOut myled(LED1);
borlanic 0:fbdae7e6d805 37 Serial pc1(USBTX, USBRX); // tx, rx
borlanic 0:fbdae7e6d805 38
borlanic 0:fbdae7e6d805 39 DigitalOut led1(PB_13);
borlanic 0:fbdae7e6d805 40 DigitalOut led2(PC_8);
borlanic 0:fbdae7e6d805 41 DigitalOut led3(PC_9);
borlanic 0:fbdae7e6d805 42 DigitalOut led4(PB_14); //neu
borlanic 0:fbdae7e6d805 43 DigitalOut led5(PB_15); // neu
borlanic 0:fbdae7e6d805 44 DigitalOut led6(PH_1);
borlanic 0:fbdae7e6d805 45 DigitalOut led7(PC_2);
borlanic 0:fbdae7e6d805 46 DigitalOut led8(PC_3);
borlanic 0:fbdae7e6d805 47 DigitalOut led9(PB_0);
borlanic 0:fbdae7e6d805 48
borlanic 0:fbdae7e6d805 49 // create motor control objects
borlanic 0:fbdae7e6d805 50 DigitalOut enable1(PB_1);
borlanic 0:fbdae7e6d805 51 DigitalOut enable2(PB_2);
borlanic 0:fbdae7e6d805 52 DigitalOut enable3(PB_3);
borlanic 0:fbdae7e6d805 53
borlanic 0:fbdae7e6d805 54 PwmOut pwm1(PA_8);
borlanic 0:fbdae7e6d805 55 PwmOut pwm2(PA_9);
borlanic 0:fbdae7e6d805 56 PwmOut pwm3(PA_10);
borlanic 0:fbdae7e6d805 57
borlanic 0:fbdae7e6d805 58 // crete Encoder read objects
borlanic 0:fbdae7e6d805 59 EncoderCounter counter1(PA_6, PC_7);
borlanic 0:fbdae7e6d805 60 EncoderCounter counter2(PB_6, PB_7);
borlanic 0:fbdae7e6d805 61 EncoderCounter counter3(PA_1, PA_0);
borlanic 0:fbdae7e6d805 62
borlanic 0:fbdae7e6d805 63 // create IMU comunication objects
borlanic 0:fbdae7e6d805 64 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
borlanic 0:fbdae7e6d805 65 DigitalOut csAG(PA_15);
borlanic 0:fbdae7e6d805 66 DigitalOut csM(PD_2);
borlanic 0:fbdae7e6d805 67
borlanic 0:fbdae7e6d805 68 // escon I/O
borlanic 0:fbdae7e6d805 69
borlanic 0:fbdae7e6d805 70
borlanic 0:fbdae7e6d805 71 AnalogIn M1_AOUT1(PC_4); // neu
borlanic 0:fbdae7e6d805 72 AnalogIn M1_AOUT2(PA_5); // neu
borlanic 0:fbdae7e6d805 73
borlanic 0:fbdae7e6d805 74 AnalogIn M2_AOUT1(PC_1); // neu
borlanic 0:fbdae7e6d805 75 AnalogIn M2_AOUT2(PC_0); // neu
borlanic 0:fbdae7e6d805 76
borlanic 0:fbdae7e6d805 77 AnalogIn M3_AOUT1(PA_7); // neu
borlanic 0:fbdae7e6d805 78 AnalogIn M3_AOUT2(PA_4); // neu
borlanic 0:fbdae7e6d805 79
borlanic 0:fbdae7e6d805 80
borlanic 0:fbdae7e6d805 81 Thread thread;
borlanic 0:fbdae7e6d805 82
borlanic 0:fbdae7e6d805 83 //IMU
borlanic 0:fbdae7e6d805 84 IMU imu(spi, csAG, csM);
borlanic 0:fbdae7e6d805 85
borlanic 0:fbdae7e6d805 86 enable1 = 1;
borlanic 0:fbdae7e6d805 87 enable2 = 1;
borlanic 0:fbdae7e6d805 88 enable3 = 1;
borlanic 0:fbdae7e6d805 89
borlanic 0:fbdae7e6d805 90 // create serial server object
borlanic 0:fbdae7e6d805 91
borlanic 0:fbdae7e6d805 92 RawSerial serial(PB_10, PC_5);
borlanic 0:fbdae7e6d805 93 serial.baud(38400);
borlanic 0:fbdae7e6d805 94 serial.format(8, SerialBase::None, 1);
borlanic 0:fbdae7e6d805 95 pc1.printf("init serilCom\r\n");
borlanic 0:fbdae7e6d805 96 SerialCom com(serial);
borlanic 0:fbdae7e6d805 97 pc1.printf("init serilCom done\r\n");
borlanic 0:fbdae7e6d805 98 pc1.printf("init Controller\r\n");
borlanic 0:fbdae7e6d805 99 //controller
borlanic 0:fbdae7e6d805 100 Controller controller(pwm1,pwm2,pwm3,counter1,counter2,counter3,imu);
borlanic 0:fbdae7e6d805 101 pc1.printf("init Controller done\r\n");
borlanic 0:fbdae7e6d805 102
borlanic 0:fbdae7e6d805 103 led1 = 0;
borlanic 0:fbdae7e6d805 104 led2 = 0;
borlanic 0:fbdae7e6d805 105 led3 = 0;
borlanic 0:fbdae7e6d805 106 led4 = 1;
borlanic 0:fbdae7e6d805 107
borlanic 0:fbdae7e6d805 108 int i = 1;
borlanic 0:fbdae7e6d805 109
borlanic 0:fbdae7e6d805 110
borlanic 0:fbdae7e6d805 111
borlanic 0:fbdae7e6d805 112 int t = 0;
borlanic 0:fbdae7e6d805 113
borlanic 0:fbdae7e6d805 114 // enter main loop
borlanic 0:fbdae7e6d805 115 pc1.printf("init done\r\n");
borlanic 0:fbdae7e6d805 116 while (true) {
borlanic 0:fbdae7e6d805 117
borlanic 0:fbdae7e6d805 118 //pc1.printf("main\r\n");
borlanic 0:fbdae7e6d805 119
borlanic 0:fbdae7e6d805 120
borlanic 0:fbdae7e6d805 121 if (com.dataRecived) {
borlanic 0:fbdae7e6d805 122 led9 = 1;
borlanic 0:fbdae7e6d805 123 controller.gainG = (1.0f+com.gainG);
borlanic 0:fbdae7e6d805 124 controller.gain_dG = (1.0f+com.gain_dG);
borlanic 0:fbdae7e6d805 125 controller.offsetX = (1.0f+com.offsetX);
borlanic 0:fbdae7e6d805 126 controller.offsetY = (1.0f+com.offsetY);
borlanic 0:fbdae7e6d805 127 com.dataRecived = false;
borlanic 0:fbdae7e6d805 128 pc1.printf("gainG = %.7f gain_dG = %.7f offsetX = %.7f offsetY = %.7f\r\n",controller.gainG,controller.gain_dG,controller.offsetX,controller.offsetY);
borlanic 0:fbdae7e6d805 129 Thread::wait(50.0f);
borlanic 0:fbdae7e6d805 130 } else {
borlanic 0:fbdae7e6d805 131 led9 = 0;
borlanic 0:fbdae7e6d805 132
borlanic 0:fbdae7e6d805 133 }
borlanic 0:fbdae7e6d805 134
borlanic 0:fbdae7e6d805 135 led7 = !led7;
borlanic 0:fbdae7e6d805 136
borlanic 0:fbdae7e6d805 137 if(M1_AOUT1.read() > M1_AOUT2.read()) {
borlanic 0:fbdae7e6d805 138 led1 = 1;
borlanic 0:fbdae7e6d805 139 } else {
borlanic 0:fbdae7e6d805 140 led1 = 0;
borlanic 0:fbdae7e6d805 141 }
borlanic 0:fbdae7e6d805 142 if(M2_AOUT1.read() > M2_AOUT2.read()) {
borlanic 0:fbdae7e6d805 143 led2 = 1;
borlanic 0:fbdae7e6d805 144 } else {
borlanic 0:fbdae7e6d805 145 led2 = 0;
borlanic 0:fbdae7e6d805 146 }
borlanic 0:fbdae7e6d805 147 if(M3_AOUT1.read() > M3_AOUT2.read()) {
borlanic 0:fbdae7e6d805 148 led3 = 1;
borlanic 0:fbdae7e6d805 149 } else {
borlanic 0:fbdae7e6d805 150 led3 = 0;
borlanic 0:fbdae7e6d805 151 }
borlanic 0:fbdae7e6d805 152
borlanic 0:fbdae7e6d805 153
borlanic 0:fbdae7e6d805 154 // security condition
borlanic 0:fbdae7e6d805 155 if(abs(imu.getGammaX()) > 0.43f || abs(imu.getGammaY()) > 0.43f) {
borlanic 0:fbdae7e6d805 156 enable1 = 0;
borlanic 0:fbdae7e6d805 157 enable2 = 0;
borlanic 0:fbdae7e6d805 158 enable3 = 0;
borlanic 0:fbdae7e6d805 159 }
borlanic 0:fbdae7e6d805 160
borlanic 0:fbdae7e6d805 161
borlanic 0:fbdae7e6d805 162 Thread::wait(50.0f);
borlanic 0:fbdae7e6d805 163 }
borlanic 0:fbdae7e6d805 164 }
borlanic 0:fbdae7e6d805 165
borlanic 0:fbdae7e6d805 166 /**
borlanic 0:fbdae7e6d805 167 * This is the main program of the ROME2RevG firmware.
borlanic 0:fbdae7e6d805 168 */
borlanic 0:fbdae7e6d805 169 int32_t main()
borlanic 0:fbdae7e6d805 170 {
borlanic 0:fbdae7e6d805 171
borlanic 0:fbdae7e6d805 172 Main main;
borlanic 0:fbdae7e6d805 173
borlanic 0:fbdae7e6d805 174 Thread::wait(osWaitForever);
borlanic 0:fbdae7e6d805 175 }
borlanic 0:fbdae7e6d805 176