BA / Mbed OS BaBoRo_test2

Backup 1

main.cpp@0:02dd72d1d465, 2018-04-24 (annotated)

Committer:
borlanic
Date:
Tue Apr 24 11:45:18 2018 +0000
Revision:
0:02dd72d1d465
BaBoRo_test2 - backup 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
borlanic 0:02dd72d1d465 1 #include "mbed.h"
borlanic 0:02dd72d1d465 2 #include "EncoderCounter.h"
borlanic 0:02dd72d1d465 3 #include "IMU.h"
borlanic 0:02dd72d1d465 4 #include "Controller.h"
borlanic 0:02dd72d1d465 5
borlanic 0:02dd72d1d465 6 DigitalOut myled(LED1);
borlanic 0:02dd72d1d465 7 Serial pc1(USBTX, USBRX); // tx, rx
borlanic 0:02dd72d1d465 8
borlanic 0:02dd72d1d465 9 DigitalOut led1(PB_13);
borlanic 0:02dd72d1d465 10 DigitalOut led2(PC_8);
borlanic 0:02dd72d1d465 11 DigitalOut led3(PC_9);
borlanic 0:02dd72d1d465 12 DigitalOut led4(PC_0);
borlanic 0:02dd72d1d465 13 DigitalOut led5(PC_1);
borlanic 0:02dd72d1d465 14 DigitalOut led6(PH_1);
borlanic 0:02dd72d1d465 15 DigitalOut led7(PC_2);
borlanic 0:02dd72d1d465 16 DigitalOut led8(PC_3);
borlanic 0:02dd72d1d465 17 DigitalOut led9(PB_0);
borlanic 0:02dd72d1d465 18
borlanic 0:02dd72d1d465 19 // create motor control objects
borlanic 0:02dd72d1d465 20 DigitalOut enable1(PB_1);
borlanic 0:02dd72d1d465 21 DigitalOut enable2(PB_2);
borlanic 0:02dd72d1d465 22 DigitalOut enable3(PB_3);
borlanic 0:02dd72d1d465 23
borlanic 0:02dd72d1d465 24 PwmOut pwm1(PA_8);
borlanic 0:02dd72d1d465 25 PwmOut pwm2(PA_9);
borlanic 0:02dd72d1d465 26 PwmOut pwm3(PA_10);
borlanic 0:02dd72d1d465 27
borlanic 0:02dd72d1d465 28 // crete Encoder read objects
borlanic 0:02dd72d1d465 29 EncoderCounter counter1(PA_6, PC_7);
borlanic 0:02dd72d1d465 30 EncoderCounter counter2(PB_6, PB_7);
borlanic 0:02dd72d1d465 31 EncoderCounter counter3(PA_1, PA_0);
borlanic 0:02dd72d1d465 32
borlanic 0:02dd72d1d465 33 // create IMU comunication objects
borlanic 0:02dd72d1d465 34 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
borlanic 0:02dd72d1d465 35 DigitalOut csAG(PA_15);
borlanic 0:02dd72d1d465 36 DigitalOut csM(PD_2);
borlanic 0:02dd72d1d465 37
borlanic 0:02dd72d1d465 38 // escon I/O
borlanic 0:02dd72d1d465 39
borlanic 0:02dd72d1d465 40
borlanic 0:02dd72d1d465 41 //AnalogIn M1_AOUT1(PB_4);
borlanic 0:02dd72d1d465 42 //AnalogIn M1_AOUT2(PB_5);
borlanic 0:02dd72d1d465 43
borlanic 0:02dd72d1d465 44 //AnalogIn M2_AOUT1(PB_14);
borlanic 0:02dd72d1d465 45 //AnalogIn M2_AOUT2(PB_15);
borlanic 0:02dd72d1d465 46
borlanic 0:02dd72d1d465 47 AnalogIn M3_AOUT1(PA_7);
borlanic 0:02dd72d1d465 48 //AnalogIn M3_AOUT2(PA_11);
borlanic 0:02dd72d1d465 49
borlanic 0:02dd72d1d465 50
borlanic 0:02dd72d1d465 51 Thread thread;
borlanic 0:02dd72d1d465 52
borlanic 0:02dd72d1d465 53 int main()
borlanic 0:02dd72d1d465 54 {
borlanic 0:02dd72d1d465 55 //IMU
borlanic 0:02dd72d1d465 56 IMU imu(spi, csAG, csM);
borlanic 0:02dd72d1d465 57
borlanic 0:02dd72d1d465 58 pwm1.period_us(200);
borlanic 0:02dd72d1d465 59 enable1 = 1;
borlanic 0:02dd72d1d465 60 //pwm1.write(0.6f);
borlanic 0:02dd72d1d465 61
borlanic 0:02dd72d1d465 62 pwm2.period_us(200);
borlanic 0:02dd72d1d465 63 enable2 = 1;
borlanic 0:02dd72d1d465 64 //pwm2.write(0.6f);
borlanic 0:02dd72d1d465 65
borlanic 0:02dd72d1d465 66 pwm3.period_us(200);
borlanic 0:02dd72d1d465 67 enable3 = 1;
borlanic 0:02dd72d1d465 68 //pwm3.write(0.6f);
borlanic 0:02dd72d1d465 69
borlanic 0:02dd72d1d465 70 //controller
borlanic 0:02dd72d1d465 71 Controller controller(pwm1,pwm2,pwm3,counter1,counter2,counter3,imu);
borlanic 0:02dd72d1d465 72
borlanic 0:02dd72d1d465 73 pc1.baud(100000);
borlanic 0:02dd72d1d465 74
borlanic 0:02dd72d1d465 75 led1 = 0;
borlanic 0:02dd72d1d465 76 led2 = 0;
borlanic 0:02dd72d1d465 77 led3 = 0;
borlanic 0:02dd72d1d465 78 led4 = 0;
borlanic 0:02dd72d1d465 79
borlanic 0:02dd72d1d465 80 int i = 1;
borlanic 0:02dd72d1d465 81
borlanic 0:02dd72d1d465 82 float gamma_z = 0;
borlanic 0:02dd72d1d465 83 float gz_vor = 0;
borlanic 0:02dd72d1d465 84
borlanic 0:02dd72d1d465 85 int t = 0;
borlanic 0:02dd72d1d465 86
borlanic 0:02dd72d1d465 87 while(1) {
borlanic 0:02dd72d1d465 88
borlanic 0:02dd72d1d465 89 led4 = 0;
borlanic 0:02dd72d1d465 90 led9 = 0;
borlanic 0:02dd72d1d465 91 led5 = 0;
borlanic 0:02dd72d1d465 92 led6 = 0;
borlanic 0:02dd72d1d465 93 led7 = 0;
borlanic 0:02dd72d1d465 94 led8 = 0;
borlanic 0:02dd72d1d465 95
borlanic 0:02dd72d1d465 96 switch(i) {
borlanic 0:02dd72d1d465 97 case 1:
borlanic 0:02dd72d1d465 98 led4 = 1;
borlanic 0:02dd72d1d465 99 break;
borlanic 0:02dd72d1d465 100
borlanic 0:02dd72d1d465 101 case 2:
borlanic 0:02dd72d1d465 102 led9 = 1;
borlanic 0:02dd72d1d465 103 break;
borlanic 0:02dd72d1d465 104
borlanic 0:02dd72d1d465 105 case 3:
borlanic 0:02dd72d1d465 106 led5 = 1;
borlanic 0:02dd72d1d465 107 break;
borlanic 0:02dd72d1d465 108
borlanic 0:02dd72d1d465 109 case 4:
borlanic 0:02dd72d1d465 110 led6 = 1;
borlanic 0:02dd72d1d465 111 break;
borlanic 0:02dd72d1d465 112
borlanic 0:02dd72d1d465 113 case 5:
borlanic 0:02dd72d1d465 114 led7 = 1;
borlanic 0:02dd72d1d465 115 break;
borlanic 0:02dd72d1d465 116
borlanic 0:02dd72d1d465 117 case 6:
borlanic 0:02dd72d1d465 118 led8 = 1;
borlanic 0:02dd72d1d465 119 break;
borlanic 0:02dd72d1d465 120
borlanic 0:02dd72d1d465 121 case 7:
borlanic 0:02dd72d1d465 122 led7 = 1;
borlanic 0:02dd72d1d465 123 break;
borlanic 0:02dd72d1d465 124
borlanic 0:02dd72d1d465 125 case 8:
borlanic 0:02dd72d1d465 126 led6 = 1;
borlanic 0:02dd72d1d465 127 break;
borlanic 0:02dd72d1d465 128
borlanic 0:02dd72d1d465 129 case 9:
borlanic 0:02dd72d1d465 130 led5 = 1;
borlanic 0:02dd72d1d465 131 break;
borlanic 0:02dd72d1d465 132 case 10:
borlanic 0:02dd72d1d465 133 led9 = 1;
borlanic 0:02dd72d1d465 134 break;
borlanic 0:02dd72d1d465 135
borlanic 0:02dd72d1d465 136 default:
borlanic 0:02dd72d1d465 137 led1 = 0;
borlanic 0:02dd72d1d465 138 led2 = 0;
borlanic 0:02dd72d1d465 139 led3 = 0;
borlanic 0:02dd72d1d465 140 led4 = 0;
borlanic 0:02dd72d1d465 141 break;
borlanic 0:02dd72d1d465 142 }
borlanic 0:02dd72d1d465 143
borlanic 0:02dd72d1d465 144 i++;
borlanic 0:02dd72d1d465 145 if(i==11) {
borlanic 0:02dd72d1d465 146 i=1;
borlanic 0:02dd72d1d465 147 }
borlanic 0:02dd72d1d465 148
borlanic 0:02dd72d1d465 149 myled = 1; // LED is ON
borlanic 0:02dd72d1d465 150 thread.wait(50.0);
borlanic 0:02dd72d1d465 151
borlanic 0:02dd72d1d465 152 float gz = imu.readGyroZ();
borlanic 0:02dd72d1d465 153 gamma_z = (gz-gz_vor)*0.05f + gamma_z;
borlanic 0:02dd72d1d465 154 float f = t*(-0.0011f);
borlanic 0:02dd72d1d465 155
borlanic 0:02dd72d1d465 156 //printf("counter1: %d counter2: %d counter3: %d\r\n", counter1.read(),counter2.read(),counter3.read());
borlanic 0:02dd72d1d465 157 //printf("%.5f %.5f\r\n",imu.getGammaX(),imu.getGammaY());
borlanic 0:02dd72d1d465 158 //printf("%.5f %.5f\r\n",imu.getGammaZ(),imu.getGammaZ());
borlanic 0:02dd72d1d465 159 //printf("%.5f %.5f\r\n",imu.readMagnetometerX()*166.0045f -550.0f, imu.readMagnetometerY()*140.6528f + 0.0f);
borlanic 0:02dd72d1d465 160 //printf("%.2f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\r\n",time,imu.readAccelerationX(), imu.readAccelerationY(), imu.readAccelerationZ(),imu.readGyroX(), imu.readGyroY(), imu.readGyroZ(),imu.readMagnetometerX(), imu.readMagnetometerY(), imu.readMagnetometerZ());
borlanic 0:02dd72d1d465 161
borlanic 0:02dd72d1d465 162 //printf("%.7f %.7f\r\n",M1_AOUT1.read(),M1_AOUT2.read());
borlanic 0:02dd72d1d465 163
borlanic 0:02dd72d1d465 164 //printf("main\r\n");
borlanic 0:02dd72d1d465 165
borlanic 0:02dd72d1d465 166 t++;
borlanic 0:02dd72d1d465 167
borlanic 0:02dd72d1d465 168 }
borlanic 0:02dd72d1d465 169 }