Artem Solomatin
fsdfds
Diff: main.cpp
- Revision:
- 2:32c9df18a589
- Parent:
- 1:36f06541698a
- Child:
- 3:8708e61475fe
--- a/main.cpp Sat May 16 12:03:46 2020 +0000
+++ b/main.cpp Sat May 16 12:09:45 2020 +0000
@@ -1,445 +0,0 @@
-#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