/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-04-20 https://www.robotway.com/

  ------------------------------*/

/*

* 舵机连接，按圆盘顺时针舵机位置依次对应Bigfish：4, 7, 11, 3, 8, 12

* 书写范围：dx --> 50; dy --> 40

*/

#include <Arduino.h>

#include <avr/pgmspace.h>

#include "model.h"

#include "words.h"

#include <Servo.h>

#define SQU 0

#define JI 1

#define QI 2

#define SHI 3

#define DAI 4

Servo myServo[SERVO_NUM];     //笔筒控制舵机数组

Servo myServo1;               //走纸舵机

Servo myServo2;               //压纸舵机

int servo_port[SERVO_NUM] = {4,7,11,3,8,12};   //笔筒控制舵机引脚   

int words_num[10] = {};   

Point squ[] = {};              //字体

Point ji[] = {};

Point qi[] = {};

Point shi[] = {};

Point dai[] = {};   

int pos_x = 0, pos_y = 1, pos_z = 2;

boolean pos_test = false;      //位置测试, 真为测试

void setup() {

  Serial.begin(9600);

  myServo1.attach(16);         //走纸舵机引脚

  myServo2.attach(17);         //压纸舵机引脚

  myServo1.write(88);          //走纸停

  pressServoDown();            //压纸

  if(pos_test) posTest();      //坐标位置测试

  wordsArrayLength();          //计算flash中存储的字体数组长度

  delay(1000);

}

void loop() {                  //写字

//   writing(SQU);

  writing(JI);                 //机

  writing(QI);                 //器

  writing(SHI);                //时

  writing(DAI);                //代

  while(1){};

}

void setPos(Point pos) {                               

  static const float _basic_dists[kActuatorCount] = {

    distance(kInitialPoint, kActuatorOrigins[0]),

    distance(kInitialPoint, kActuatorOrigins[1]),

    distance(kInitialPoint, kActuatorOrigins[2]),

    distance(kInitialPoint, kActuatorOrigins[3]),

    distance(kInitialPoint, kActuatorOrigins[4]),

    distance(kInitialPoint, kActuatorOrigins[5])

  };

  int degree[kActuatorCount] = {};

  for (int i = 0; i < kActuatorCount; ++i)

  {

    float dist = distance(pos, kActuatorOrigins[i]);

    float deg = 90.0 + (dist - _basic_dists[i]) /

                   kActuatorRadius / M_PI * 180.0;

    degree[i] = floor(deg+0.5);

  }

  for (int i = 0; i < kActuatorCount; ++i)

  {

    ServoGo(i, map(degree[i], 0, 180, 700, 2100));

  }

}

void writeLine(Point a, Point b, unsigned long t) {

  static const int dt = 50;

  unsigned long k = t / dt;

  float dx = (b.x - a.x) / (float)k;

  float dy = (b.y - a.y) / (float)k;

  Point p = a;

  for (int i = 0; i < k; ++i)

  {

    setPos(p);

    delay(dt);

    p.x += dx;

    p.y += dy;

  }

}

void ServoStart(int which)

{

  if(!myServo[which].attached())myServo[which].attach(servo_port[which]);

  pinMode(servo_port[which], OUTPUT);

}

void ServoStop(int which)

{

  myServo[which].detach();

  digitalWrite(servo_port[which],LOW);

}

void ServoGo(int which , int where)

{

    ServoStart(which);

    myServo[which].writeMicroseconds(where);

}

void posTest(){

//    setPos({175, 185, 20});   //最小坐标

//    delay(1000);

    setPos({200, 200, 50});   //中间坐标,确定笔的位置时可将位置设置为中间坐标，打开开关然后机械调整舵机角度直到笔筒在中间位置即可

    delay(1000);

//    setPos({225, 225, 20});   //最大坐标

//    delay(1000);

    while(1){delay(10);};

}

void wordsArrayLength(){

   words_num[0] = sizeof(squArray) / sizeof(squArray[0]) / 3;

   words_num[1] = sizeof(jiArray) / sizeof(jiArray[0]) / 3;             //机

   words_num[2] = sizeof(qiArray) / sizeof(qiArray[0]) / 3;             //器

   words_num[3] = sizeof(shiArray) / sizeof(shiArray[0]) / 3;           //时

   words_num[4] = sizeof(daiArray) / sizeof(daiArray[0]) / 3;           //代

}

void readProgmem(int p, Point a, Point b, int ary[]){

    a.x = pgm_read_word_near( ary + p * 3 + pos_x) + 175;

    a.y = pgm_read_word_near( ary + p * 3 + pos_y) + 185;

    a.z = pgm_read_word_near( ary + p * 3 + pos_z);

    b.x = pgm_read_word_near( ary + (p + 1) * 3 + pos_x) + 175;

    b.y = pgm_read_word_near( ary + (p + 1) * 3 + pos_y) + 185;

    b.z = pgm_read_word_near( ary + (p + 1) * 3 + pos_z);

    writeLine(a, b, 500);

//    Serial.print(a.x);  

//    Serial.print("_");

//    Serial.print(a.y);

//    Serial.print(" ");

//    Serial.print(b.x);  

//    Serial.print("_");

//    Serial.println(b.y);

}

void writing(int which){

  for(int i=0;i<words_num[which] - 1;i++){

    switch(which){

        case 0:

          readProgmem(i, squ[i], squ[i+1], squArray);

        break;

        case 1:

          readProgmem(i, ji[i], ji[i+1], jiArray);

        break;

        case 2:

          readProgmem(i, qi[i], qi[i+1], qiArray);

        break;

        case 3:

          readProgmem(i, shi[i], shi[i+1], shiArray);

        break;

        case 4:

          readProgmem(i, dai[i], dai[i+1], daiArray);

        break;

    }

    pos_x = 0;

    pos_y = 1;

    pos_z = 2;

  }

  setPos({200, 200, 50});

  delay(1000);

  pressServoUp();

  delay(100);

  positionSwitch();

  pressServoDown();

}

void positionSwitch(){                //走纸函数

    myServo1.write(80);

    delay(500);

    myServo1.write(88);

    delay(100);

}

void pressServoDown(){                //压杆落

  myServo2.write(70);  

}

void pressServoUp(){                  //压杆抬

  myServo2.write(75);  

}