Artem Solomatin
fsdfds
Diff: main.cpp
- Revision:
- 1:36f06541698a
- Parent:
- 0:bdfad365ab0b
- Child:
- 2:32c9df18a589
diff -r bdfad365ab0b -r 36f06541698a main.cpp
--- a/main.cpp Sat May 16 12:02:45 2020 +0000
+++ b/main.cpp Sat May 16 12:03:46 2020 +0000
@@ -0,0 +1,445 @@
+#include "mbed.h"
+
+#define M_PI 3.14159265358979323846f
+#define smoothingNumber 6
+
+#define STATE_ERROR 0
+#define STATE_INIT 1
+#define STATE_GOTO_START 2
+#define STATE_GOTO_END_COUNTING 3
+#define STATE_GOTO_MIDDLE 4
+#define STATE_WAITING 5
+#define STATE_GOTO_SWING 6
+#define STATE_START_SWING 7
+#define STATE_SWING_RIGHT 8
+#define STATE_SWING_LEFT 9
+#define STATE_EMPTY_SWING 10
+
+#define DIR_LEFT 0
+#define DIR_RIGHT 1
+
+//Serial pc(D1,D0,4800);
+DigitalOut RCout(D10);
+DigitalOut dir(D9);
+InterruptIn leftSwitch(D2);
+InterruptIn rightSwitch(D3);
+Ticker tick;
+Ticker ledTicker;
+DigitalOut myled(LED2);
+RawSerial rpc(D1,D0,9600);
+
+int period_us = 26;//300 26
+
+bool isSwinged = false;
+
+long pos=0;
+
+long railLength = 0;
+
+uint8_t state=STATE_INIT;
+
+Timer t;
+SPI spi(D11,D12,D13);// mosi, miso, sclk
+DigitalOut cs(D5);
+
+int driveSpeed = 0;
+
+float angularPosNew = 0;
+float angularPosOld = 0;
+
+float timeOld = 0.0f;
+float timeStart = 0.0f;
+
+float dx = 0;
+float xPosNew = 0;
+float xPosOld = 0;
+
+double dAngle = 0.0f;
+double anSpd = 0.0f;
+
+float timeOldPos = 0.0f;
+float timeStartPos = 0.0f;
+
+float angle=0.f;
+float angleOffset = 0;
+
+double PIPI = 6.28;
+
+bool canSend = false;
+
+typedef union {
+ float number[5];
+ uint8_t numberCh[20];
+} my_union;
+
+my_union myUnion;
+
+bool isPendulumSwinging() {
+ return state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT;
+}
+
+float getPosMM() {
+ //return (pos-railLength/2) * 550.0f/railLength;
+ //return (pos - railLength / 2) * (350.0f / railLength);
+ return pos / 3500.0f;
+}
+
+uint16_t getPendulumPos(){
+ cs=0;
+ wait_ms(1);
+ uint16_t d=spi.write((short)0x00);
+ d=d<<1;//fucking shithole fakebit
+ d=d>>6;//no need debug info
+ cs=1;
+ wait_ms(1);
+ return (uint16_t)d;
+}
+
+
+float getPendulumAngle(){
+ angle = getPendulumPos();
+ angle = angle * 6.28f / 1024.0f;
+ return angle;
+}
+
+
+/*
+ ANGULAR SPEED CALC
+*/
+
+void getDeltaAng(){
+
+ angularPosNew = getPendulumAngle();
+
+ dAngle = fmod((angularPosNew - angularPosOld + 3.14), PIPI) - 3.14;
+ if (dAngle < -3.14) {
+ dAngle += PIPI;
+ }
+
+ angularPosOld = angularPosNew;
+
+}
+
+void getAngularSpeed(){
+ float deltaTime;
+
+ timeStart = float(t.read());
+ deltaTime = (timeStart - timeOld);
+ getDeltaAng();
+ anSpd = dAngle / deltaTime;
+ timeOld=timeStart;
+ //взятие по модулю, спросить
+}
+
+/*
+ SPEED CALC
+*/
+
+void getDeltaPos()
+{
+ float delta = 0;
+ xPosNew = getPosMM();
+ delta = xPosNew - xPosOld;
+ dx = delta;
+ xPosOld = xPosNew;
+}
+
+float getSpeed(){
+ float deltaTime;
+ float speed;
+ getDeltaPos();
+ timeStartPos = float(t.read());
+ deltaTime = (timeStartPos - timeOldPos);
+ speed = (dx) * deltaTime;
+ timeOldPos = timeStartPos;
+ //взятие по модулю, спросить
+ return speed;
+}
+
+
+
+void stepperFlip() {
+ if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){
+ RCout = !RCout;
+ pos += (dir.read() * 2 - 1);
+ }
+ if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) {
+ state = STATE_WAITING;
+ }
+ if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
+ pos += (dir.read() * 2 - 1);
+ RCout = !RCout;
+ }
+}
+
+
+
+void led() {
+ myled = !myled;
+}
+
+void updateBlink(uint64_t t){
+ ledTicker.detach();
+ ledTicker.attach_us (&led, t / 2);
+}
+
+void updatePeriod(){
+ tick.detach();
+ tick.attach_us (&stepperFlip, period_us / 2.0f);
+}
+
+void leftEnd() {
+ dir = DIR_RIGHT;
+ if (state == STATE_GOTO_START) {
+ state = STATE_GOTO_END_COUNTING;
+ pos = 0;
+ }
+ else if (isPendulumSwinging()) {
+ state = STATE_GOTO_MIDDLE;
+ //angleOffset -= 0.006191;
+ //state = STATE_ERROR;
+ }
+ //при втыкании в концевик меняем смещение (offset)
+}
+
+void rightEnd() {
+ dir=DIR_LEFT;
+ if (state == STATE_GOTO_END_COUNTING) {
+ railLength=pos;
+ state = STATE_GOTO_MIDDLE;
+ }
+ else if (isPendulumSwinging()) {
+ state = STATE_GOTO_MIDDLE;
+ //angleOffset += 0.006191;
+ //state = STATE_ERROR;
+ }
+}
+
+void getSwingDirectory() {
+ /* if (getPendulumAngle() >= 0) {
+ state = STATE_SWING_RIGHT;
+ dir = DIR_LEFT;
+ } else if (getPendulumAngle() * 100 < 0) {
+ state = STATE_SWING_LEFT;
+ dir = DIR_RIGHT;
+ } else if (getPendulumAngle() * 100 > 10) {
+ if (getPendulumAngle() * 100 < 0) {
+
+ } else {
+ state = STATE_SWING_LEFT;
+ dir = DIR_RIGHT;
+ }
+ } else if (getPendulumAngle() == 0.006191) {
+ state = STATE_SWING_RIGHT;
+ dir = DIR_LEFT;
+ }
+ return ;*/
+
+ //ебля с угловой скоростью и положением
+ /*
+ if (int(getPendulumAngle()* 1000) == -12 && int(getAngularSpeed() * 1000000) < 3) {
+ state = STATE_SWING_LEFT;
+ dir = DIR_RIGHT;
+ } else if (int(getPendulumAngle() * 1000) == 6 && int(getAngularSpeed() * 1000000) < 3) {
+ state = STATE_SWING_RIGHT;
+ dir = DIR_LEFT;
+ } else if (getPendulumAngle() == 0 && int(getAngularSpeed() * 1000000) < 3) {
+ state = STATE_SWING_RIGHT;
+ dir = DIR_LEFT;
+ } else if (int(getPendulumAngle() * 1000) == -6 && int(getAngularSpeed() * 1000000) < 3) {
+ state = STATE_SWING_LEFT;
+ dir = DIR_RIGHT;
+ } */
+ int currentSpd = anSpd;
+ /*if (currentSpd >= 0 && currentSpd <= 1000000000) {
+ state = STATE_SWING_LEFT;
+ dir = DIR_LEFT;
+ return;
+ }
+ if (currentSpd >= -1000000000 && currentSpd <= ) {
+ state = STATE_SWING_RIGHT;
+ dir = DIR_RIGHT;
+ return;
+ }*/
+ // printf("angl %f spd %f crspd %d\r\n", angle, getAngularSpeed(), currentSpd);
+ if (currentSpd > 0 ) {
+ state = STATE_SWING_RIGHT;
+ dir = DIR_LEFT;
+ //dir = DIR_LEFT;
+ return;
+ }
+ if (currentSpd < 0) {
+ state = STATE_SWING_LEFT;
+ dir = DIR_RIGHT;
+ //dir = DIR_RIGHT;
+ return;
+ }
+}
+
+void resumeSwinging(){
+ if (state == STATE_EMPTY_SWING) {
+ /*getSwingDirectory();
+ if (state == STATE_SWING_LEFT) {
+ dir = DIR_LEFT;
+ state = STATE_SWING_RIGHT;
+ } else if (state == STATE_SWING_RIGHT) {
+ dir = DIR_RIGHT;
+ state = STATE_SWING_LEFT;
+ }*/
+ state = STATE_GOTO_MIDDLE;
+ }
+ }
+ /*
+void initialSwinging() {
+ swingTimer.start();
+ while(swingTimer.read() < 5)
+ {
+ updateBlink(13);
+ printf("hi");
+ if (getPendulumAngle() >= 0) {
+ dir = DIR_LEFT;
+ } else if (getPendulumAngle() * 100 < 0) {
+ dir = DIR_RIGHT;
+ } else if (getPendulumAngle() * 100 > 10) {
+ if (getPendulumAngle() * 100 < 0) {
+ } else {
+ dir = DIR_RIGHT;
+ }
+ } else if (getPendulumAngle() == 0.006191) {
+ dir = DIR_LEFT;
+ }
+ }
+ swingTimer.stop();
+ isSwinged = true;
+ getSwingDirectory();
+}*/
+
+void sendData() {
+ getAngularSpeed();
+ myUnion.number[0] = t.read();
+ myUnion.number[1] = dx;
+ myUnion.number[2] = getSpeed();
+ myUnion.number[3] = dAngle;
+ myUnion.number[4] = anSpd;
+ for(int i = 0; i < 20; i++) {
+ rpc.putc(myUnion.numberCh[i]);
+ }
+}
+
+void Rx_interrupt() {
+ int password = rpc.getc();
+ if (password == 228) {
+ canSend = true;
+ }
+ return;
+}
+
+int main() {
+ RCout = 1;
+ wait_ms(500);
+ t.start();
+ leftSwitch.rise(&leftEnd);
+ rightSwitch.rise(&rightEnd);
+ for (int i=5; i>0; i--) {
+ getPendulumAngle();
+ myled = !myled;
+ wait_ms(500);
+ }
+ //angleOffset=angle;
+ //angleOffset = 0.006191;
+ //angleOffset = 0;
+ /*for (int i=5; i>0; i--) {
+ getPendulumAngle();
+ myled = !myled;
+ wait_ms(500);
+ }
+ angleOffset=angle;*/
+ updatePeriod();
+ state=STATE_GOTO_START;
+ dir=DIR_LEFT;
+ spi.format(16,2);
+ spi.frequency(1000000);
+ rpc.attach(&Rx_interrupt, Serial::RxIrq);
+ while(1) {
+ //angleOffset = 0.006191;
+ // printf("st: %d - %d, pos: %d is: %f spd: %d\r\n", state, railLength, pos,getPendulumAngle(), int(getAngularSpeed() * 1000000));
+ // printf("delta: %f, angle: %f spd %d\r\n",deltaPos(), getPendulumAngle(), int(getAngularSpeed() * 1000000));
+
+
+ //rpc.printf("%f %f %f %f %f\n",dx, getSpeed(), angle , getAngularSpeed(), t.read());
+ if(canSend) {
+ sendData();
+ } else {
+ getSpeed();
+ getAngularSpeed();
+ }
+ //printf("pos %f spd %f time %f\n", getPosMM(), getSpeed(), t.read());
+
+ //printf("%d\n", pos);
+ // printf("angl %f spd %f\r\n",angle, getAngularSpeed());
+ switch(state) {
+ case STATE_WAITING:
+ //updateBlink(13400);
+ //wait(1);
+ // updateBlink(25000);
+ updateBlink(1000);
+ /*if (isSwinged)
+ {
+ state = STATE_GOTO_SWING;
+ }
+ initialSwinging();*/
+ state = STATE_GOTO_SWING;
+ break;
+ case STATE_GOTO_START:
+ //updateBlink(25000);
+ updateBlink(1000);
+ break;
+ case STATE_GOTO_END_COUNTING:
+ //updateBlink(25000);
+ updateBlink(1000);
+ break;
+ case STATE_GOTO_SWING:
+ //updateBlink(25000);
+ updateBlink(1000);
+ // dir = DIR_LEFT;
+ //getSwingDirectory();
+ //updateBlink(50);
+ getSwingDirectory();
+ //printf("off %f\n", angleOffset);
+ break;
+ case STATE_SWING_LEFT:
+
+ getSwingDirectory();
+ //printf("off %f\n", angleOffset);
+ /*if (pos > 55000) {
+ state = STATE_GOTO_MIDDLE;
+ dir=DIR_LEFT;
+ } else if(pos < 5000) {
+ state = STATE_GOTO_MIDDLE;
+ dir=DIR_RIGHT;
+ }*/
+ break;
+ case STATE_SWING_RIGHT:
+
+ getSwingDirectory();
+ //printf("off %f\n", angleOffset);
+ /* if (pos < 5000) {
+ state = STATE_GOTO_MIDDLE;
+ dir=DIR_RIGHT;
+ } else if(pos > 55000) {
+ state = STATE_GOTO_MIDDLE;
+ dir=DIR_LEFT;
+ }*/
+ break;
+ /*
+ case STATE_EMPTY_SWING:
+ resumeSwinging();
+ break;*/
+ default:
+ //updateBlink(25000);
+ updateBlink(1000);
+ break;
+ }
+ //wait_ms(5);
+ }
+}
\ No newline at end of file