Robotics Studio / Mbed 2 deprecated NucleoBoard_1

Board1

Dependencies:   mbed Map

main.cpp@1:4eeae0f92c4c, 2019-04-20 (annotated)

Committer:
pbdt1997
Date:
Sat Apr 20 07:02:28 2019 +0000
Revision:
1:4eeae0f92c4c
Parent:
0:97879c8efe59
Child:
2:2ef793d0ac01
Master Board;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pbdt1997 0:97879c8efe59 1 //Module8-9
pbdt1997 0:97879c8efe59 2 //Nucleo Board 1 (Master)
pbdt1997 0:97879c8efe59 3
pbdt1997 0:97879c8efe59 4 #include "mbed.h"
pbdt1997 0:97879c8efe59 5 #include <Map.hpp>
pbdt1997 0:97879c8efe59 6
pbdt1997 0:97879c8efe59 7 Serial pc(USBTX, USBRX);
pbdt1997 0:97879c8efe59 8
pbdt1997 0:97879c8efe59 9 SPI master(PA_7, PA_6, PA_5); //SPI1_MOSI, SPI1_MISO, SPI1_SCLK
pbdt1997 1:4eeae0f92c4c 10 SPI mstrEnc2(PB_15, PB_14, PB_13); //SPI2_MOSI, SPI2_MISO, SPI2_SCLK
pbdt1997 1:4eeae0f92c4c 11 SPI mstrEnc1(PC_12, PC_11, PC_10); //SPI3_MOSI, SPI3_MISO, SPI3_SCLK
pbdt1997 0:97879c8efe59 12
pbdt1997 0:97879c8efe59 13 DigitalOut csMstr(PA_15); //SPI1_SSEL Master
pbdt1997 1:4eeae0f92c4c 14 DigitalOut csEnc2(PB_12); //SPI2_SSEL Encoder 1
pbdt1997 1:4eeae0f92c4c 15 DigitalOut csEnc1(PA_4); //SPI3_SSEL Encoder 2
pbdt1997 0:97879c8efe59 16
pbdt1997 1:4eeae0f92c4c 17 DigitalOut dir1(PC_0);
pbdt1997 1:4eeae0f92c4c 18 PwmOut pwm1(PC_9);
pbdt1997 1:4eeae0f92c4c 19
pbdt1997 1:4eeae0f92c4c 20 DigitalOut dir2(PC_1);
pbdt1997 1:4eeae0f92c4c 21 PwmOut pwm2(PC_8);
pbdt1997 0:97879c8efe59 22
pbdt1997 0:97879c8efe59 23 //Parameters
pbdt1997 0:97879c8efe59 24
pbdt1997 0:97879c8efe59 25 char array[4];
pbdt1997 0:97879c8efe59 26 char arraySize = sizeof(array);
pbdt1997 0:97879c8efe59 27 char getData = 0;
pbdt1997 0:97879c8efe59 28 char checkData = 0;
pbdt1997 0:97879c8efe59 29 char encArr[2]= {255,255};
pbdt1997 0:97879c8efe59 30 char encLength = 2;
pbdt1997 0:97879c8efe59 31 bool start = false;
pbdt1997 0:97879c8efe59 32
pbdt1997 1:4eeae0f92c4c 33
pbdt1997 0:97879c8efe59 34 /////////////////////////////
pbdt1997 0:97879c8efe59 35
pbdt1997 0:97879c8efe59 36 //Functions
pbdt1997 0:97879c8efe59 37
pbdt1997 1:4eeae0f92c4c 38 //store data from pc
pbdt1997 0:97879c8efe59 39 void RX(int data){
pbdt1997 0:97879c8efe59 40 array[checkData] = data;
pbdt1997 0:97879c8efe59 41 checkData++;
pbdt1997 0:97879c8efe59 42 if(checkData == arraySize){
pbdt1997 0:97879c8efe59 43 getData = 1;
pbdt1997 0:97879c8efe59 44 checkData = 0;
pbdt1997 0:97879c8efe59 45 }
pbdt1997 0:97879c8efe59 46 }
pbdt1997 0:97879c8efe59 47
pbdt1997 1:4eeae0f92c4c 48 //receive data from pc
pbdt1997 0:97879c8efe59 49 void RX_INT(){
pbdt1997 0:97879c8efe59 50 int data = pc.getc();
pbdt1997 0:97879c8efe59 51 RX(data);
pbdt1997 0:97879c8efe59 52 }
pbdt1997 0:97879c8efe59 53
pbdt1997 1:4eeae0f92c4c 54 //convert 14bit data to angle
pbdt1997 0:97879c8efe59 55 float ConvertAngle(int encData){
pbdt1997 0:97879c8efe59 56 Map map(0, 16383, 0, 359);
pbdt1997 0:97879c8efe59 57 float angle = map.Calculate(encData);
pbdt1997 0:97879c8efe59 58 return angle;
pbdt1997 0:97879c8efe59 59 }
pbdt1997 0:97879c8efe59 60
pbdt1997 1:4eeae0f92c4c 61 //read encoder of joint 1
pbdt1997 0:97879c8efe59 62 float readEnc1(){
pbdt1997 0:97879c8efe59 63 csEnc1 = 0;
pbdt1997 0:97879c8efe59 64 char *arrAdd = &encArr[0];
pbdt1997 0:97879c8efe59 65 int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2);
pbdt1997 0:97879c8efe59 66 encData = encArr[0] * 256 + encArr[1];
pbdt1997 0:97879c8efe59 67 csEnc1 = 1;
pbdt1997 0:97879c8efe59 68 return ConvertAngle(encData/2);
pbdt1997 0:97879c8efe59 69 }
pbdt1997 0:97879c8efe59 70
pbdt1997 1:4eeae0f92c4c 71 //read encoder of joint 2
pbdt1997 0:97879c8efe59 72 float readEnc2(){
pbdt1997 0:97879c8efe59 73 csEnc2 = 0;
pbdt1997 0:97879c8efe59 74 char *arrAdd = &encArr[0];
pbdt1997 0:97879c8efe59 75 int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2);
pbdt1997 0:97879c8efe59 76 encData = encArr[0] * 256 + encArr[1];
pbdt1997 0:97879c8efe59 77 csEnc2 = 1;
pbdt1997 0:97879c8efe59 78 return ConvertAngle(encData/2);
pbdt1997 0:97879c8efe59 79 }
pbdt1997 0:97879c8efe59 80
pbdt1997 1:4eeae0f92c4c 81 //drive motor joint1
pbdt1997 1:4eeae0f92c4c 82 void drvMotor1(float duty){
pbdt1997 1:4eeae0f92c4c 83 if(duty > 0.9){
pbdt1997 1:4eeae0f92c4c 84 duty = 0.9;
pbdt1997 1:4eeae0f92c4c 85 }
pbdt1997 1:4eeae0f92c4c 86 else if(duty < -0.9){
pbdt1997 1:4eeae0f92c4c 87 duty = -0.9;
pbdt1997 1:4eeae0f92c4c 88 }
pbdt1997 1:4eeae0f92c4c 89 if(duty >= 0){
pbdt1997 1:4eeae0f92c4c 90 dir1 = 1; //CW
pbdt1997 1:4eeae0f92c4c 91
pbdt1997 1:4eeae0f92c4c 92 }
pbdt1997 1:4eeae0f92c4c 93 else if(duty < 0){
pbdt1997 1:4eeae0f92c4c 94 dir1 = 0; //CCW
pbdt1997 1:4eeae0f92c4c 95 duty = -duty;
pbdt1997 1:4eeae0f92c4c 96 }
pbdt1997 1:4eeae0f92c4c 97 pwm1.period_ms(1.0f);
pbdt1997 1:4eeae0f92c4c 98 pwm1.write(duty);
pbdt1997 1:4eeae0f92c4c 99 }
pbdt1997 1:4eeae0f92c4c 100
pbdt1997 1:4eeae0f92c4c 101 //drive motor joint1
pbdt1997 1:4eeae0f92c4c 102 void drvMotor2(float duty){
pbdt1997 1:4eeae0f92c4c 103 if(duty > 0.5){
pbdt1997 1:4eeae0f92c4c 104 duty = 0.5;
pbdt1997 1:4eeae0f92c4c 105 }
pbdt1997 1:4eeae0f92c4c 106 else if(duty < -0.5){
pbdt1997 1:4eeae0f92c4c 107 duty = -0.5;
pbdt1997 1:4eeae0f92c4c 108 }
pbdt1997 1:4eeae0f92c4c 109 if(duty >= 0){
pbdt1997 1:4eeae0f92c4c 110 dir2 = 1; //CCW
pbdt1997 1:4eeae0f92c4c 111 }
pbdt1997 1:4eeae0f92c4c 112 else if(duty < 0){
pbdt1997 1:4eeae0f92c4c 113 dir2 = 0; //CW
pbdt1997 1:4eeae0f92c4c 114 duty = -duty;
pbdt1997 1:4eeae0f92c4c 115 }
pbdt1997 1:4eeae0f92c4c 116 pwm2.period_ms(1.0f);
pbdt1997 1:4eeae0f92c4c 117 pwm2.write(duty);
pbdt1997 1:4eeae0f92c4c 118 }
pbdt1997 1:4eeae0f92c4c 119
pbdt1997 1:4eeae0f92c4c 120 //PID of joint 1
pbdt1997 1:4eeae0f92c4c 121 void PID_1(float pos){
pbdt1997 1:4eeae0f92c4c 122 pos = -pos;
pbdt1997 1:4eeae0f92c4c 123 const float Kp=0.5,Ki=0,Kd=0.1;
pbdt1997 1:4eeae0f92c4c 124 float u,e,p,s;
pbdt1997 1:4eeae0f92c4c 125 float offset = 110.66;
pbdt1997 1:4eeae0f92c4c 126 bool stop1 = false;
pbdt1997 1:4eeae0f92c4c 127
pbdt1997 1:4eeae0f92c4c 128 while(stop1==false){
pbdt1997 1:4eeae0f92c4c 129 float encData1 = readEnc1();
pbdt1997 1:4eeae0f92c4c 130 float encData2 = readEnc2();
pbdt1997 1:4eeae0f92c4c 131 printf("Encoder1: %.2f ", encData1);
pbdt1997 1:4eeae0f92c4c 132 printf("Encoder2: %.2f\n", encData2);
pbdt1997 1:4eeae0f92c4c 133 if(encData1 < 110){
pbdt1997 1:4eeae0f92c4c 134 encData1 = encData1 + 360;
pbdt1997 1:4eeae0f92c4c 135 }
pbdt1997 1:4eeae0f92c4c 136 e = (pos - encData1) + offset;
pbdt1997 1:4eeae0f92c4c 137 s = s + e;
pbdt1997 1:4eeae0f92c4c 138 if(abs(e)>0.5){
pbdt1997 1:4eeae0f92c4c 139 u = (Kp*e) + (Ki*s) + (Kd*(e-p));
pbdt1997 1:4eeae0f92c4c 140 }
pbdt1997 1:4eeae0f92c4c 141 else{
pbdt1997 1:4eeae0f92c4c 142 u = 0;
pbdt1997 1:4eeae0f92c4c 143 drvMotor1(0);
pbdt1997 1:4eeae0f92c4c 144 stop1 = true;
pbdt1997 1:4eeae0f92c4c 145 }
pbdt1997 1:4eeae0f92c4c 146 drvMotor1(u);
pbdt1997 1:4eeae0f92c4c 147 p = e;
pbdt1997 1:4eeae0f92c4c 148 }
pbdt1997 1:4eeae0f92c4c 149 }
pbdt1997 1:4eeae0f92c4c 150
pbdt1997 1:4eeae0f92c4c 151 //PID of joint 2
pbdt1997 1:4eeae0f92c4c 152 void PID_2(float pos){
pbdt1997 1:4eeae0f92c4c 153 const float Kp=1,Ki=0,Kd=0.1;
pbdt1997 1:4eeae0f92c4c 154 float u,e,p,s;
pbdt1997 1:4eeae0f92c4c 155 float offset = 21.52 - 135;
pbdt1997 1:4eeae0f92c4c 156 bool stop = false;
pbdt1997 1:4eeae0f92c4c 157 pos=-pos;
pbdt1997 1:4eeae0f92c4c 158 while(stop==false){
pbdt1997 1:4eeae0f92c4c 159 float encData1 = readEnc1();
pbdt1997 1:4eeae0f92c4c 160 float encData2 = readEnc2();
pbdt1997 1:4eeae0f92c4c 161 printf("Encoder1: %.2f ", encData1);
pbdt1997 1:4eeae0f92c4c 162 printf("Encoder2: %.2f\n", encData2);
pbdt1997 1:4eeae0f92c4c 163 e = (pos - encData2) + offset;
pbdt1997 1:4eeae0f92c4c 164 s = s + e;
pbdt1997 1:4eeae0f92c4c 165 if(abs(e)>0.5){
pbdt1997 1:4eeae0f92c4c 166 u = (Kp*e) + (Ki*s) + (Kd*(e-p));
pbdt1997 1:4eeae0f92c4c 167 }
pbdt1997 1:4eeae0f92c4c 168 else{
pbdt1997 1:4eeae0f92c4c 169 u = 0;
pbdt1997 1:4eeae0f92c4c 170 drvMotor2(0);
pbdt1997 1:4eeae0f92c4c 171 stop = true;
pbdt1997 1:4eeae0f92c4c 172 }
pbdt1997 1:4eeae0f92c4c 173 drvMotor2(u);
pbdt1997 1:4eeae0f92c4c 174 p = e;
pbdt1997 1:4eeae0f92c4c 175 }
pbdt1997 1:4eeae0f92c4c 176 }
pbdt1997 1:4eeae0f92c4c 177 //Set Home
pbdt1997 1:4eeae0f92c4c 178 void setHome(){
pbdt1997 1:4eeae0f92c4c 179 PID_1(0);
pbdt1997 1:4eeae0f92c4c 180 PID_2(0);
pbdt1997 1:4eeae0f92c4c 181 }
pbdt1997 1:4eeae0f92c4c 182
pbdt1997 0:97879c8efe59 183 //Main
pbdt1997 0:97879c8efe59 184
pbdt1997 0:97879c8efe59 185 int main() {
pbdt1997 1:4eeae0f92c4c 186 pc.baud(256000);
pbdt1997 0:97879c8efe59 187 RX_INT();
pbdt1997 0:97879c8efe59 188 pc.attach(&RX_INT, Serial::RxIrq);
pbdt1997 0:97879c8efe59 189
pbdt1997 0:97879c8efe59 190 csMstr = 1;
pbdt1997 0:97879c8efe59 191 csEnc1 = 1;
pbdt1997 0:97879c8efe59 192 csEnc2 = 1;
pbdt1997 0:97879c8efe59 193
pbdt1997 0:97879c8efe59 194 master.format(8,3);
pbdt1997 0:97879c8efe59 195 master.frequency(1000000);
pbdt1997 0:97879c8efe59 196 mstrEnc1.format(14, 3);
pbdt1997 0:97879c8efe59 197 mstrEnc1.frequency(500000);
pbdt1997 0:97879c8efe59 198 mstrEnc2.format(14, 3);
pbdt1997 0:97879c8efe59 199 mstrEnc2.frequency(500000);
pbdt1997 0:97879c8efe59 200
pbdt1997 0:97879c8efe59 201 //Program Loop
pbdt1997 0:97879c8efe59 202 while(true){
pbdt1997 0:97879c8efe59 203 if(getData == 1){
pbdt1997 0:97879c8efe59 204 for(char i = 0; i < arraySize; i++){
pbdt1997 0:97879c8efe59 205 pc.putc(array[i]);
pbdt1997 0:97879c8efe59 206 }
pbdt1997 0:97879c8efe59 207 if(array[2] == 122){ //receive 'z'
pbdt1997 0:97879c8efe59 208 start = true;
pbdt1997 0:97879c8efe59 209 csMstr = 0;
pbdt1997 0:97879c8efe59 210 master.write(array[3]);
pbdt1997 0:97879c8efe59 211 int masterData = master.write(0x00);
pbdt1997 0:97879c8efe59 212 csMstr = 1;
pbdt1997 0:97879c8efe59 213 pc.putc(masterData);
pbdt1997 0:97879c8efe59 214 pc.putc('A');
pbdt1997 1:4eeae0f92c4c 215 }
pbdt1997 1:4eeae0f92c4c 216 if(array[2] == 121){ //'y'
pbdt1997 1:4eeae0f92c4c 217 setHome();
pbdt1997 0:97879c8efe59 218 }
pbdt1997 0:97879c8efe59 219
pbdt1997 0:97879c8efe59 220 getData = 0;
pbdt1997 0:97879c8efe59 221 }
pbdt1997 1:4eeae0f92c4c 222 //test driving joint 1 and joint 2
pbdt1997 1:4eeae0f92c4c 223 PID_2(45);
pbdt1997 1:4eeae0f92c4c 224 wait(0.5);
pbdt1997 1:4eeae0f92c4c 225 PID_1(90);
pbdt1997 1:4eeae0f92c4c 226 wait(0.1);
pbdt1997 1:4eeae0f92c4c 227 PID_2(30);
pbdt1997 1:4eeae0f92c4c 228 wait(3);
pbdt1997 1:4eeae0f92c4c 229 PID_1(0);
pbdt1997 1:4eeae0f92c4c 230 wait(0.1);
pbdt1997 1:4eeae0f92c4c 231 PID_2(-45);
pbdt1997 1:4eeae0f92c4c 232 wait(100);
pbdt1997 0:97879c8efe59 233
pbdt1997 0:97879c8efe59 234 }
pbdt1997 0:97879c8efe59 235
pbdt1997 0:97879c8efe59 236
pbdt1997 0:97879c8efe59 237
pbdt1997 0:97879c8efe59 238
pbdt1997 0:97879c8efe59 239 }