Robotics Studio
/
NucleoBoard_1
Board1
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?
User | Revision | Line number | New 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 | } |