Artem Solomatin / Mbed 2 deprecated encoderRot

fcvb vb

Dependencies:   mbed

main.cpp@1:85ab39fea863, 2020-05-16 (annotated)

Committer:
teamat
Date:
Sat May 16 15:08:46 2020 +0000
Revision:
1:85ab39fea863
Parent:
0:1cc40073c858 
qewqe

Who changed what in which revision?

UserRevisionLine numberNew contents of line
teamat 0:1cc40073c858 1 #include "mbed.h"
teamat 0:1cc40073c858 2 #define smoothingNumber 3
teamat 0:1cc40073c858 3 #define M_PI 3.14159265358979323846f
teamat 0:1cc40073c858 4
teamat 0:1cc40073c858 5 Timer t;
teamat 0:1cc40073c858 6 SPI spi(D11,D12,D13);// mosi, miso, sclk
teamat 0:1cc40073c858 7 DigitalOut cs(D5);
teamat 0:1cc40073c858 8
teamat 0:1cc40073c858 9 InterruptIn leftSwitch(D2);
teamat 0:1cc40073c858 10 InterruptIn rightSwitch(D3);
teamat 0:1cc40073c858 11
teamat 0:1cc40073c858 12 int driveSpeed = 0;
teamat 0:1cc40073c858 13
teamat 0:1cc40073c858 14 double angularPosNew = 0;
teamat 0:1cc40073c858 15 double angularPosOld = 0;
teamat 0:1cc40073c858 16 double dAngle = 0.0f;
teamat 0:1cc40073c858 17 double PIPI = 6.28;
teamat 0:1cc40073c858 18 float anSpd = 0.0f;
teamat 0:1cc40073c858 19 float prevAnSpd = 0.0f;
teamat 0:1cc40073c858 20 float beforeOffset = 0;
teamat 0:1cc40073c858 21
teamat 0:1cc40073c858 22 float timeOld = 0.0f;
teamat 0:1cc40073c858 23 float timeStart = 0.0f;
teamat 1:85ab39fea863 24 float proceeded = 0.0f;
teamat 1:85ab39fea863 25 float test= 0.0f;
teamat 0:1cc40073c858 26
teamat 0:1cc40073c858 27 void Rx_interrupt();
teamat 0:1cc40073c858 28
teamat 0:1cc40073c858 29 bool canSend = false;
teamat 0:1cc40073c858 30
teamat 0:1cc40073c858 31 RawSerial rpc(D1,D0,9600);
teamat 0:1cc40073c858 32
teamat 0:1cc40073c858 33 typedef union {
teamat 0:1cc40073c858 34 float number[6];
teamat 0:1cc40073c858 35 uint8_t numberCh[24];
teamat 0:1cc40073c858 36 } my_union;
teamat 0:1cc40073c858 37
teamat 0:1cc40073c858 38 my_union myUnion;
teamat 0:1cc40073c858 39
teamat 0:1cc40073c858 40 uint16_t getPendulumPos(){
teamat 0:1cc40073c858 41 cs=0;
teamat 0:1cc40073c858 42 wait_ms(1);
teamat 0:1cc40073c858 43 uint16_t d=spi.write((short)0x00);
teamat 0:1cc40073c858 44 d=d<<1;//fucking shithole fakebit
teamat 0:1cc40073c858 45 d=d>>6;//no need debug info
teamat 0:1cc40073c858 46 cs=1;
teamat 0:1cc40073c858 47 wait_ms(1);
teamat 0:1cc40073c858 48 return (uint16_t)d;
teamat 0:1cc40073c858 49 }
teamat 0:1cc40073c858 50
teamat 0:1cc40073c858 51
teamat 0:1cc40073c858 52 float angle=0.f;
teamat 0:1cc40073c858 53 float angleOffset=0;
teamat 0:1cc40073c858 54
teamat 0:1cc40073c858 55 float getPendulumAngle(){
teamat 0:1cc40073c858 56 angle = getPendulumPos();
teamat 0:1cc40073c858 57 angle = angle * 6.28f / 1024.0f;
teamat 0:1cc40073c858 58 beforeOffset = angle;
teamat 1:85ab39fea863 59 angle += angleOffset;
teamat 1:85ab39fea863 60 /*if (angle >= angleOffset) {
teamat 1:85ab39fea863 61 angle += angleOffset;
teamat 0:1cc40073c858 62 } else {
teamat 0:1cc40073c858 63 //angle += (angleOffset + 0.244346);
teamat 1:85ab39fea863 64 //angle += angleOffset + 0.23970866018;
teamat 1:85ab39fea863 65 //angle += angleOffset;
teamat 1:85ab39fea863 66 angle -= angleOffset;
teamat 1:85ab39fea863 67 }*/
teamat 1:85ab39fea863 68 if (angle > PIPI + 0.01) {
teamat 1:85ab39fea863 69 proceeded = fmod((angle + 3.14), PIPI) - 3.14;
teamat 1:85ab39fea863 70 if (proceeded < -3.14) {
teamat 1:85ab39fea863 71 proceeded += PIPI;
teamat 1:85ab39fea863 72 }
teamat 1:85ab39fea863 73 /* if (proceeded < 0) {
teamat 1:85ab39fea863 74 proceeded = -proceeded;
teamat 1:85ab39fea863 75 }*/
teamat 1:85ab39fea863 76 angle = proceeded;
teamat 0:1cc40073c858 77 }
teamat 1:85ab39fea863 78 test = fmodf(angle,M_PI);
teamat 1:85ab39fea863 79 if (test > 1) {
teamat 1:85ab39fea863 80 test = -test;
teamat 1:85ab39fea863 81 }
teamat 1:85ab39fea863 82
teamat 0:1cc40073c858 83 return angle;
teamat 0:1cc40073c858 84 }
teamat 0:1cc40073c858 85
teamat 0:1cc40073c858 86 void deltaPos(){
teamat 0:1cc40073c858 87
teamat 0:1cc40073c858 88 /*
teamat 0:1cc40073c858 89 for(int i=0;i<smoothingNumber;i++){
teamat 0:1cc40073c858 90 angularPosNew+=getPendulumAngle();
teamat 0:1cc40073c858 91 //wait_ms(1);
teamat 0:1cc40073c858 92 }
teamat 0:1cc40073c858 93 angularPosNew/=smoothingNumber;
teamat 0:1cc40073c858 94
teamat 0:1cc40073c858 95 */
teamat 0:1cc40073c858 96 angularPosNew = getPendulumAngle();
teamat 0:1cc40073c858 97
teamat 0:1cc40073c858 98 dAngle = fmod((angularPosNew - angularPosOld + 3.14), PIPI) - 3.14;
teamat 0:1cc40073c858 99 if (dAngle < -3.14) {
teamat 0:1cc40073c858 100 dAngle += PIPI;
teamat 0:1cc40073c858 101 }
teamat 0:1cc40073c858 102
teamat 0:1cc40073c858 103 /*
teamat 0:1cc40073c858 104 if (dAngle < 0) {
teamat 0:1cc40073c858 105 dAngle += 6.34;
teamat 0:1cc40073c858 106 }
teamat 0:1cc40073c858 107 if (dAngle > M_PI) {
teamat 0:1cc40073c858 108 dAngle -= 6.34;
teamat 0:1cc40073c858 109 }
teamat 0:1cc40073c858 110 */
teamat 0:1cc40073c858 111 angularPosOld = angularPosNew;
teamat 0:1cc40073c858 112 }
teamat 0:1cc40073c858 113
teamat 0:1cc40073c858 114
teamat 0:1cc40073c858 115
teamat 0:1cc40073c858 116 void angularSpeed(){
teamat 0:1cc40073c858 117 float deltaTime;
teamat 0:1cc40073c858 118 timeStart = float(t.read());
teamat 0:1cc40073c858 119 deltaTime = (timeStart - timeOld);
teamat 0:1cc40073c858 120 deltaPos();
teamat 0:1cc40073c858 121 anSpd = (dAngle) / deltaTime;
teamat 0:1cc40073c858 122 /*
teamat 0:1cc40073c858 123 int test = anSpd;
teamat 0:1cc40073c858 124 if (test == 0) {
teamat 0:1cc40073c858 125 anSpd = 0;
teamat 0:1cc40073c858 126 }*/
teamat 0:1cc40073c858 127 timeOld=timeStart;
teamat 0:1cc40073c858 128 }
teamat 0:1cc40073c858 129
teamat 0:1cc40073c858 130
teamat 0:1cc40073c858 131 void Rx_interrupt() {
teamat 0:1cc40073c858 132
teamat 0:1cc40073c858 133 int password = rpc.getc();
teamat 0:1cc40073c858 134 if (password == 50) {
teamat 0:1cc40073c858 135 canSend = true;
teamat 0:1cc40073c858 136 }
teamat 0:1cc40073c858 137
teamat 0:1cc40073c858 138 return;
teamat 0:1cc40073c858 139 }
teamat 0:1cc40073c858 140
teamat 0:1cc40073c858 141 void leftEnd() {
teamat 0:1cc40073c858 142 }
teamat 0:1cc40073c858 143
teamat 0:1cc40073c858 144
teamat 0:1cc40073c858 145 void rightEnd() {
teamat 0:1cc40073c858 146 }
teamat 0:1cc40073c858 147
teamat 0:1cc40073c858 148 void sendData() {
teamat 0:1cc40073c858 149 angularSpeed();
teamat 0:1cc40073c858 150 myUnion.number[0] = t.read();
teamat 0:1cc40073c858 151 myUnion.number[1] = angleOffset;
teamat 0:1cc40073c858 152 myUnion.number[2] = beforeOffset;
teamat 0:1cc40073c858 153 myUnion.number[3] = 228.0;
teamat 0:1cc40073c858 154 myUnion.number[4] = angularPosNew;
teamat 1:85ab39fea863 155 myUnion.number[5] = test;
teamat 0:1cc40073c858 156 for(int i = 0 ; i < 24; i++) {
teamat 0:1cc40073c858 157 rpc.putc(myUnion.numberCh[i]);
teamat 0:1cc40073c858 158 }
teamat 0:1cc40073c858 159 }
teamat 0:1cc40073c858 160
teamat 0:1cc40073c858 161 int main() {
teamat 0:1cc40073c858 162 leftSwitch.rise(&leftEnd);
teamat 0:1cc40073c858 163 rightSwitch.rise(&rightEnd);
teamat 0:1cc40073c858 164 spi.format(16,2);
teamat 0:1cc40073c858 165 spi.frequency(1000000);
teamat 0:1cc40073c858 166 t.start();
teamat 0:1cc40073c858 167 for (int i=5; i>0; i--) {
teamat 0:1cc40073c858 168 getPendulumAngle();
teamat 0:1cc40073c858 169 wait_ms(500);
teamat 0:1cc40073c858 170 }
teamat 1:85ab39fea863 171 angleOffset= 3.14 - angle;
teamat 0:1cc40073c858 172 rpc.attach(&Rx_interrupt, Serial::RxIrq);
teamat 0:1cc40073c858 173 while(1) {
teamat 0:1cc40073c858 174 wait_us(5);
teamat 0:1cc40073c858 175 if (canSend) {
teamat 0:1cc40073c858 176 sendData();
teamat 0:1cc40073c858 177 }
teamat 0:1cc40073c858 178 //wait_ms(8);
teamat 0:1cc40073c858 179 //angleOffset=angle;
teamat 0:1cc40073c858 180 //printf("deltaPos: %f\r\nspeed: %f\r\n",getPendulumAngle(),angularSpeed());
teamat 0:1cc40073c858 181 //printf("dPos: %f, without %f spd: %f \r\n",getPendulumAngle(), getPendulumAngle() - 0.006191, angularSpeed());
teamat 0:1cc40073c858 182 //printf("speed: %f\r\n",angularSpeed());
teamat 0:1cc40073c858 183 }
teamat 0:1cc40073c858 184 }