Robotics Studio
/
NucleoBoard_1
Board1
Diff: main.cpp
- Revision:
- 1:4eeae0f92c4c
- Parent:
- 0:97879c8efe59
- Child:
- 2:2ef793d0ac01
diff -r 97879c8efe59 -r 4eeae0f92c4c main.cpp --- a/main.cpp Wed Mar 27 14:09:53 2019 +0000 +++ b/main.cpp Sat Apr 20 07:02:28 2019 +0000 @@ -7,13 +7,18 @@ Serial pc(USBTX, USBRX); SPI master(PA_7, PA_6, PA_5); //SPI1_MOSI, SPI1_MISO, SPI1_SCLK -SPI mstrEnc1(PB_15, PB_14, PB_13); //SPI2_MOSI, SPI2_MISO, SPI2_SCLK -SPI mstrEnc2(PC_12, PC_11, PC_10); //SPI3_MOSI, SPI3_MISO, SPI3_SCLK +SPI mstrEnc2(PB_15, PB_14, PB_13); //SPI2_MOSI, SPI2_MISO, SPI2_SCLK +SPI mstrEnc1(PC_12, PC_11, PC_10); //SPI3_MOSI, SPI3_MISO, SPI3_SCLK DigitalOut csMstr(PA_15); //SPI1_SSEL Master -DigitalOut csEnc1(PB_12); //SPI2_SSEL Encoder 1 -DigitalOut csEnc2(PA_4); //SPI3_SSEL Encoder 2 +DigitalOut csEnc2(PB_12); //SPI2_SSEL Encoder 1 +DigitalOut csEnc1(PA_4); //SPI3_SSEL Encoder 2 +DigitalOut dir1(PC_0); +PwmOut pwm1(PC_9); + +DigitalOut dir2(PC_1); +PwmOut pwm2(PC_8); //Parameters @@ -25,10 +30,12 @@ char encLength = 2; bool start = false; + ///////////////////////////// //Functions +//store data from pc void RX(int data){ array[checkData] = data; checkData++; @@ -38,19 +45,21 @@ } } +//receive data from pc void RX_INT(){ int data = pc.getc(); RX(data); } +//convert 14bit data to angle float ConvertAngle(int encData){ Map map(0, 16383, 0, 359); float angle = map.Calculate(encData); return angle; } +//read encoder of joint 1 float readEnc1(){ - //Encoder1 = LOW csEnc1 = 0; char *arrAdd = &encArr[0]; int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2); @@ -59,8 +68,8 @@ return ConvertAngle(encData/2); } +//read encoder of joint 2 float readEnc2(){ - //Encoder2 = LOW csEnc2 = 0; char *arrAdd = &encArr[0]; int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2); @@ -69,10 +78,112 @@ return ConvertAngle(encData/2); } +//drive motor joint1 +void drvMotor1(float duty){ + if(duty > 0.9){ + duty = 0.9; + } + else if(duty < -0.9){ + duty = -0.9; + } + if(duty >= 0){ + dir1 = 1; //CW + + } + else if(duty < 0){ + dir1 = 0; //CCW + duty = -duty; + } + pwm1.period_ms(1.0f); + pwm1.write(duty); +} + +//drive motor joint1 +void drvMotor2(float duty){ + if(duty > 0.5){ + duty = 0.5; + } + else if(duty < -0.5){ + duty = -0.5; + } + if(duty >= 0){ + dir2 = 1; //CCW + } + else if(duty < 0){ + dir2 = 0; //CW + duty = -duty; + } + pwm2.period_ms(1.0f); + pwm2.write(duty); +} + +//PID of joint 1 +void PID_1(float pos){ + pos = -pos; + const float Kp=0.5,Ki=0,Kd=0.1; + float u,e,p,s; + float offset = 110.66; + bool stop1 = false; + + while(stop1==false){ + float encData1 = readEnc1(); + float encData2 = readEnc2(); + printf("Encoder1: %.2f ", encData1); + printf("Encoder2: %.2f\n", encData2); + if(encData1 < 110){ + encData1 = encData1 + 360; + } + e = (pos - encData1) + offset; + s = s + e; + if(abs(e)>0.5){ + u = (Kp*e) + (Ki*s) + (Kd*(e-p)); + } + else{ + u = 0; + drvMotor1(0); + stop1 = true; + } + drvMotor1(u); + p = e; + } +} + +//PID of joint 2 +void PID_2(float pos){ + const float Kp=1,Ki=0,Kd=0.1; + float u,e,p,s; + float offset = 21.52 - 135; + bool stop = false; + pos=-pos; + while(stop==false){ + float encData1 = readEnc1(); + float encData2 = readEnc2(); + printf("Encoder1: %.2f ", encData1); + printf("Encoder2: %.2f\n", encData2); + e = (pos - encData2) + offset; + s = s + e; + if(abs(e)>0.5){ + u = (Kp*e) + (Ki*s) + (Kd*(e-p)); + } + else{ + u = 0; + drvMotor2(0); + stop = true; + } + drvMotor2(u); + p = e; + } +} +//Set Home +void setHome(){ + PID_1(0); + PID_2(0); + } + //Main int main() { - pc.baud(9600); + pc.baud(256000); RX_INT(); pc.attach(&RX_INT, Serial::RxIrq); @@ -87,16 +198,10 @@ mstrEnc2.format(14, 3); mstrEnc2.frequency(500000); - //Program Loop while(true){ if(getData == 1){ for(char i = 0; i < arraySize; i++){ - //printf("array[%d] = %d\n", i,array[i]); -// csMstr = 0; -// master.write(array[i]); -// int masterData = master.write(0x00); -// printf("masterData = %d\n", masterData); pc.putc(array[i]); } if(array[2] == 122){ //receive 'z' @@ -104,23 +209,27 @@ csMstr = 0; master.write(array[3]); int masterData = master.write(0x00); - //printf("masterData = %d\n", masterData); - //state = 0; csMstr = 1; pc.putc(masterData); pc.putc('A'); - -// wait(1); + } + if(array[2] == 121){ //'y' + setHome(); } getData = 0; } - if(start == true){ - float encData1 = readEnc1(); - float encData2 = readEnc2(); - printf("Encoder1 = %.2f ", encData1); - printf("Encoder2 = %.2f \n\r", encData2); - } + //test driving joint 1 and joint 2 + PID_2(45); + wait(0.5); + PID_1(90); + wait(0.1); + PID_2(30); + wait(3); + PID_1(0); + wait(0.1); + PID_2(-45); + wait(100); }