Artem Solomatin / Mbed 2 deprecated encoderRot

fcvb vb

Dependencies:   mbed

main.cpp@0:1cc40073c858, 2020-05-16 (annotated)

Committer:
teamat
Date:
Sat May 16 12:36:40 2020 +0000
Revision:
0:1cc40073c858
Child:
1:85ab39fea863
vddsffds

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 0:1cc40073c858 24
teamat 0:1cc40073c858 25 void Rx_interrupt();
teamat 0:1cc40073c858 26
teamat 0:1cc40073c858 27 bool canSend = false;
teamat 0:1cc40073c858 28
teamat 0:1cc40073c858 29 RawSerial rpc(D1,D0,9600);
teamat 0:1cc40073c858 30
teamat 0:1cc40073c858 31 typedef union {
teamat 0:1cc40073c858 32 float number[6];
teamat 0:1cc40073c858 33 uint8_t numberCh[24];
teamat 0:1cc40073c858 34 } my_union;
teamat 0:1cc40073c858 35
teamat 0:1cc40073c858 36 my_union myUnion;
teamat 0:1cc40073c858 37
teamat 0:1cc40073c858 38 uint16_t getPendulumPos(){
teamat 0:1cc40073c858 39 cs=0;
teamat 0:1cc40073c858 40 wait_ms(1);
teamat 0:1cc40073c858 41 uint16_t d=spi.write((short)0x00);
teamat 0:1cc40073c858 42 d=d<<1;//fucking shithole fakebit
teamat 0:1cc40073c858 43 d=d>>6;//no need debug info
teamat 0:1cc40073c858 44 cs=1;
teamat 0:1cc40073c858 45 wait_ms(1);
teamat 0:1cc40073c858 46 return (uint16_t)d;
teamat 0:1cc40073c858 47 }
teamat 0:1cc40073c858 48
teamat 0:1cc40073c858 49
teamat 0:1cc40073c858 50 float angle=0.f;
teamat 0:1cc40073c858 51 float angleOffset=0;
teamat 0:1cc40073c858 52
teamat 0:1cc40073c858 53 float getPendulumAngle(){
teamat 0:1cc40073c858 54 angle = getPendulumPos();
teamat 0:1cc40073c858 55 angle = angle * 6.28f / 1024.0f;
teamat 0:1cc40073c858 56 beforeOffset = angle;
teamat 0:1cc40073c858 57 if (angle >= angleOffset) {
teamat 0:1cc40073c858 58 angle -= angleOffset;
teamat 0:1cc40073c858 59 } else {
teamat 0:1cc40073c858 60 //angle += (angleOffset + 0.244346);
teamat 0:1cc40073c858 61 angle += angleOffset + 0.23970866018;
teamat 0:1cc40073c858 62 }
teamat 0:1cc40073c858 63 return angle;
teamat 0:1cc40073c858 64 }
teamat 0:1cc40073c858 65
teamat 0:1cc40073c858 66 void deltaPos(){
teamat 0:1cc40073c858 67
teamat 0:1cc40073c858 68 /*
teamat 0:1cc40073c858 69 for(int i=0;i<smoothingNumber;i++){
teamat 0:1cc40073c858 70 angularPosNew+=getPendulumAngle();
teamat 0:1cc40073c858 71 //wait_ms(1);
teamat 0:1cc40073c858 72 }
teamat 0:1cc40073c858 73 angularPosNew/=smoothingNumber;
teamat 0:1cc40073c858 74
teamat 0:1cc40073c858 75 */
teamat 0:1cc40073c858 76 angularPosNew = getPendulumAngle();
teamat 0:1cc40073c858 77
teamat 0:1cc40073c858 78 dAngle = fmod((angularPosNew - angularPosOld + 3.14), PIPI) - 3.14;
teamat 0:1cc40073c858 79 if (dAngle < -3.14) {
teamat 0:1cc40073c858 80 dAngle += PIPI;
teamat 0:1cc40073c858 81 }
teamat 0:1cc40073c858 82
teamat 0:1cc40073c858 83 /*
teamat 0:1cc40073c858 84 if (dAngle < 0) {
teamat 0:1cc40073c858 85 dAngle += 6.34;
teamat 0:1cc40073c858 86 }
teamat 0:1cc40073c858 87 if (dAngle > M_PI) {
teamat 0:1cc40073c858 88 dAngle -= 6.34;
teamat 0:1cc40073c858 89 }
teamat 0:1cc40073c858 90 */
teamat 0:1cc40073c858 91 angularPosOld = angularPosNew;
teamat 0:1cc40073c858 92 }
teamat 0:1cc40073c858 93
teamat 0:1cc40073c858 94
teamat 0:1cc40073c858 95
teamat 0:1cc40073c858 96 void angularSpeed(){
teamat 0:1cc40073c858 97 float deltaTime;
teamat 0:1cc40073c858 98 timeStart = float(t.read());
teamat 0:1cc40073c858 99 deltaTime = (timeStart - timeOld);
teamat 0:1cc40073c858 100 deltaPos();
teamat 0:1cc40073c858 101 anSpd = (dAngle) / deltaTime;
teamat 0:1cc40073c858 102 /*
teamat 0:1cc40073c858 103 int test = anSpd;
teamat 0:1cc40073c858 104 if (test == 0) {
teamat 0:1cc40073c858 105 anSpd = 0;
teamat 0:1cc40073c858 106 }*/
teamat 0:1cc40073c858 107 timeOld=timeStart;
teamat 0:1cc40073c858 108 }
teamat 0:1cc40073c858 109
teamat 0:1cc40073c858 110
teamat 0:1cc40073c858 111 void Rx_interrupt() {
teamat 0:1cc40073c858 112
teamat 0:1cc40073c858 113 int password = rpc.getc();
teamat 0:1cc40073c858 114 if (password == 50) {
teamat 0:1cc40073c858 115 canSend = true;
teamat 0:1cc40073c858 116 }
teamat 0:1cc40073c858 117
teamat 0:1cc40073c858 118 return;
teamat 0:1cc40073c858 119 }
teamat 0:1cc40073c858 120
teamat 0:1cc40073c858 121 void leftEnd() {
teamat 0:1cc40073c858 122 }
teamat 0:1cc40073c858 123
teamat 0:1cc40073c858 124
teamat 0:1cc40073c858 125 void rightEnd() {
teamat 0:1cc40073c858 126 }
teamat 0:1cc40073c858 127
teamat 0:1cc40073c858 128 void sendData() {
teamat 0:1cc40073c858 129 angularSpeed();
teamat 0:1cc40073c858 130 myUnion.number[0] = t.read();
teamat 0:1cc40073c858 131 myUnion.number[1] = angleOffset;
teamat 0:1cc40073c858 132 myUnion.number[2] = beforeOffset;
teamat 0:1cc40073c858 133 myUnion.number[3] = 228.0;
teamat 0:1cc40073c858 134 myUnion.number[4] = angularPosNew;
teamat 0:1cc40073c858 135 myUnion.number[5] = anSpd;
teamat 0:1cc40073c858 136 for(int i = 0 ; i < 24; i++) {
teamat 0:1cc40073c858 137 rpc.putc(myUnion.numberCh[i]);
teamat 0:1cc40073c858 138 }
teamat 0:1cc40073c858 139 }
teamat 0:1cc40073c858 140
teamat 0:1cc40073c858 141 int main() {
teamat 0:1cc40073c858 142 leftSwitch.rise(&leftEnd);
teamat 0:1cc40073c858 143 rightSwitch.rise(&rightEnd);
teamat 0:1cc40073c858 144 spi.format(16,2);
teamat 0:1cc40073c858 145 spi.frequency(1000000);
teamat 0:1cc40073c858 146 t.start();
teamat 0:1cc40073c858 147 for (int i=5; i>0; i--) {
teamat 0:1cc40073c858 148 getPendulumAngle();
teamat 0:1cc40073c858 149 wait_ms(500);
teamat 0:1cc40073c858 150 }
teamat 0:1cc40073c858 151 angleOffset=angle;
teamat 0:1cc40073c858 152 rpc.attach(&Rx_interrupt, Serial::RxIrq);
teamat 0:1cc40073c858 153 while(1) {
teamat 0:1cc40073c858 154 wait_us(5);
teamat 0:1cc40073c858 155 if (canSend) {
teamat 0:1cc40073c858 156 sendData();
teamat 0:1cc40073c858 157 }
teamat 0:1cc40073c858 158 //wait_ms(8);
teamat 0:1cc40073c858 159 //angleOffset=angle;
teamat 0:1cc40073c858 160 //printf("deltaPos: %f\r\nspeed: %f\r\n",getPendulumAngle(),angularSpeed());
teamat 0:1cc40073c858 161 //printf("dPos: %f, without %f spd: %f \r\n",getPendulumAngle(), getPendulumAngle() - 0.006191, angularSpeed());
teamat 0:1cc40073c858 162 //printf("speed: %f\r\n",angularSpeed());
teamat 0:1cc40073c858 163 }
teamat 0:1cc40073c858 164 }